4-bar system simulation#
This notebook builds upon the system-level simulation showcased in the system-level notebook. If you are not already familiar with the concepts described in that notebook, please go through it before this one.
import numpy as np
import Karana.Dynamics as kd
import Karana.Dynamics.SOADyn_types as kdt
from Karana.KUtils.Sim import SimDS
import Karana.Models as kmdl
import Karana.Integrators as ki
import Karana.KUtils as ku
import Karana.Core as kc
import atexit
import Karana.Math as km
from Karana.Math.Kquantities import ureg
from pathlib import Path
import runpy
from tempfile import NamedTemporaryFile
Create the same serial chain simulation and populate the multibody using a DataStruct (DS) file just as before.
sim_ds_path = Path("..") / "resources" / "4_bar" / "4_bar.h5"
sim = SimDS.fromFile(sim_ds_path).toSim()
sim.mb.resetData()
sim.mb.getScene().update()
[WebUI] Web server is running on port 36019
You may be able to connect in your browser at:
http://newton:36019
[WebUI] Karana Viewer is running on port 29534
Open this URL to connect:
http://newton:29534
While you can examine the new model in the gui, we also do so via the command line below.
sim.mb.displayModel()
LEGEND: <body number> <body name> <parent body> <hinge type> [prescribed subhinges] <U dofs/offset> [flex dofs/offset]
Body Parent Hinge Dofs
____ ______ _____ ____
1. bd1 multibody_MBVROOT_ REVOLUTE 1/0
2. bd2 bd1 REVOLUTE 1/1
3. bd3 bd2 REVOLUTE 1/2
____
3
Nodes
---
cn <ConstraintNode> body=bd3
Enabled cutjoint loop constraints <1, residuals=5, error=6>
---------------
constraint <REVOLUTE> source=bd3/cn target=ROOT/multibody_MBVROOT_
sim.mb.dumpTree()
LEGEND: [<hinge type[/dofs]> <prescribed subhinges>] <body name>[/num embedded bodies > 0] <flex dofs > 0>
|-multibody_MBVROOT_
|-[REVOLUTE] bd1
|-[REVOLUTE] bd2
|-[REVOLUTE] bd3 [---> lc/REVOLUTE ---> ROOT]
Initial state#
Similar to before, we’ll use the Python API to set a legal initial position and velocity for the system that satisfies the constraints. We begin by assigning initial coordinate values.
sim.mb.getBody("bd1").parentHinge().subhinge(0).setQ(0.25)
sim.mb.getBody("bd1").parentHinge().subhinge(0).setU(1.0)
sim.mb.getBody("bd2").parentHinge().subhinge(0).setQ(1.25)
sim.mb.getBody("bd3").parentHinge().subhinge(0).setQ(1.25)
True
Not surprisingly, these initial coordinate values do not satisfy the constraint as revealed by the following error checks which return non-zero error values.
sim.mb.poseError()
array([-0. , -2.75 , -0. , -0.62315438, -0. ,
0.13251775])
sim.mb.velError()
array([ 0. , -1. , 0. , 0.11534724, 0. ,
-1.62655994])
To remedy this situation, we use the built in constraint kinematics solver (CKS) to solve for coordinates and states that do satisfy the constraints.
assert sim.mb.cks().solveQ() < 1e-8
assert sim.mb.cks().solveU() < 1e-8
sim.mb.getScene().viewAroundFrame(
sim.mb.virtualRoot(), np.array([0.0, 5.0, 0.0]), np.zeros(3), np.array([0.0, 0.0, 1.0])
)
# Update the Integrator with these values
sim.sp.ensureHealthy()
sim.sp.hardReset()
sim.sp.setState(sim.sp.assembleState())
Register a Timed Event#
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 for more information.
sp = sim.sp
def fn(t):
"""Print out the time and state.
Parameters
----------
t : float
The current time.
"""
print(f"t = {float(sp.getTime()) / 1e9}s; x = {np.round(sp.getIntegrator().getX(), 6)}")
h = np.timedelta64(100, "ms")
t = kd.TimedEvent("hop_size", h, fn, False)
t.period = h
# register the timed event
sim.sp.registerTimedEvent(t)
del t
Add plots#
Use the addPlot helper method on the sim to add some plots to the GUI.
import platform
if platform.system() != "Darwin":
# In the Jupyter notebook, the live plotting does not work on macOS due to multiprocessing issues.
# This works just fine from a regular Python script if guarded by if __name__ == "__main__"
sim.addPlots(
[
ku.SinglePlotData(
"Energy",
sim.sp.getVars().time,
[
(
"Energy",
lambda: np.array(
[
kd.Algorithms.evalKineticEnergy(sim.mb)
+ kd.Algorithms.evalGravitationalPotentialEnergy(sim.mb)
]
),
1,
)
],
)
],
0.1,
title="Time domain plots",
)
Dash app started at http://newton:45265
Run the simulation#
Now, let’s run the simulation. advanceTo will advance to the desired time.
sim.sp.advanceTo(10)
t = 0.1s; x = [-0.295796 1.866593 1.275 -2.845796 0.25 -0.25 0.25
-0.25 ]
t = 0.2s; x = [-0.270796 1.841593 1.3 -2.870796 0.25 -0.25 0.25
-0.25 ]
t = 0.3s; x = [-0.245796 1.816593 1.325 -2.895796 0.25 -0.25 0.25
-0.25 ]
t = 0.4s; x = [-0.220796 1.791593 1.35 -2.920796 0.25 -0.25 0.25
-0.25 ]
t = 0.5s; x = [-0.195796 1.766593 1.375 -2.945796 0.25 -0.25 0.25
-0.25 ]
t = 0.6s; x = [-0.170796 1.741593 1.4 -2.970796 0.25 -0.25 0.25
-0.25 ]
t = 0.7s; x = [-0.145796 1.716593 1.425 -2.995796 0.25 -0.25 0.25
-0.25 ]
t = 0.8s; x = [-0.120796 1.691593 1.45 -3.020796 0.25 -0.25 0.25
-0.25 ]
t = 0.9s; x = [-0.095796 1.666593 1.475 -3.045796 0.25 -0.25 0.25
-0.25 ]
t = 1.0s; x = [-0.070796 1.641593 1.5 -3.070796 0.25 -0.25 0.25
-0.25 ]
t = 1.1s; x = [-0.045796 1.616593 1.525 -3.095796 0.25 -0.25 0.25
-0.25 ]
t = 1.2s; x = [-0.020796 1.591593 1.55 -3.120796 0.25 -0.25 0.25
-0.25 ]
t = 1.3s; x = [ 0.004204 1.566593 1.575 -3.145796 0.25 -0.25 0.25
-0.25 ]
t = 1.4s; x = [ 0.029204 1.541593 1.6 -3.170796 0.25 -0.25 0.25
-0.25 ]
t = 1.5s; x = [ 0.054204 1.516593 1.625 -3.195796 0.25 -0.25 0.25
-0.25 ]
t = 1.6s; x = [ 0.079204 1.491593 1.65 -3.220796 0.25 -0.25 0.25
-0.25 ]
t = 1.7s; x = [ 0.104204 1.466593 1.675 -3.245796 0.25 -0.25 0.25
-0.25 ]
t = 1.8s; x = [ 0.129204 1.441593 1.7 -3.270796 0.25 -0.25 0.25
-0.25 ]
t = 1.9s; x = [ 0.154204 1.416593 1.725 -3.295796 0.25 -0.25 0.25
-0.25 ]
t = 2.0s; x = [ 0.179204 1.391593 1.75 -3.320796 0.25 -0.25 0.25
-0.25 ]
t = 2.1s; x = [ 0.204204 1.366593 1.775 -3.345796 0.25 -0.25 0.25
-0.25 ]
t = 2.2s; x = [ 0.229204 1.341593 1.8 -3.370796 0.25 -0.25 0.25
-0.25 ]
t = 2.3s; x = [ 0.254204 1.316593 1.825 -3.395796 0.25 -0.25 0.25
-0.25 ]
t = 2.4s; x = [ 0.279204 1.291593 1.85 -3.420796 0.25 -0.25 0.25
-0.25 ]
t = 2.5s; x = [ 0.304204 1.266593 1.875 -3.445796 0.25 -0.25 0.25
-0.25 ]
t = 2.6s; x = [ 0.329204 1.241593 1.9 -3.470796 0.25 -0.25 0.25
-0.25 ]
t = 2.7s; x = [ 0.354204 1.216593 1.925 -3.495796 0.25 -0.25 0.25
-0.25 ]
t = 2.8s; x = [ 0.379204 1.191593 1.95 -3.520796 0.25 -0.25 0.25
-0.25 ]
t = 2.9s; x = [ 0.404204 1.166593 1.975 -3.545796 0.25 -0.25 0.25
-0.25 ]
t = 3.0s; x = [ 0.429204 1.141593 2. -3.570796 0.25 -0.25 0.25
-0.25 ]
t = 3.1s; x = [ 0.454204 1.116593 2.025 -3.595796 0.25 -0.25 0.25
-0.25 ]
t = 3.2s; x = [ 0.479204 1.091593 2.05 -3.620796 0.25 -0.25 0.25
-0.25 ]
t = 3.3s; x = [ 0.504204 1.066593 2.075 -3.645796 0.25 -0.25 0.25
-0.25 ]
t = 3.4s; x = [ 0.529204 1.041593 2.1 -3.670796 0.25 -0.25 0.25
-0.25 ]
t = 3.5s; x = [ 0.554204 1.016593 2.125 -3.695796 0.25 -0.25 0.25
-0.25 ]
t = 3.6s; x = [ 0.579204 0.991593 2.15 -3.720796 0.25 -0.25 0.25
-0.25 ]
t = 3.7s; x = [ 0.604204 0.966593 2.175 -3.745796 0.25 -0.25 0.25
-0.25 ]
t = 3.8s; x = [ 0.629204 0.941593 2.2 -3.770796 0.25 -0.25 0.25
-0.25 ]
t = 3.9s; x = [ 0.654204 0.916593 2.225 -3.795796 0.25 -0.25 0.25
-0.25 ]
t = 4.0s; x = [ 0.679204 0.891593 2.25 -3.820796 0.25 -0.25 0.25
-0.25 ]
t = 4.1s; x = [ 0.704204 0.866593 2.275 -3.845796 0.25 -0.25 0.25
-0.25 ]
t = 4.2s; x = [ 0.729204 0.841593 2.3 -3.870796 0.25 -0.25 0.25
-0.25 ]
t = 4.3s; x = [ 0.754204 0.816593 2.325 -3.895796 0.25 -0.25 0.25
-0.25 ]
t = 4.4s; x = [ 0.779204 0.791593 2.35 -3.920796 0.25 -0.25 0.25
-0.25 ]
t = 4.5s; x = [ 0.804204 0.766593 2.375 -3.945796 0.25 -0.25 0.25
-0.25 ]
t = 4.6s; x = [ 0.829204 0.741593 2.4 -3.970796 0.25 -0.25 0.25
-0.25 ]
t = 4.7s; x = [ 0.854204 0.716593 2.425 -3.995796 0.25 -0.25 0.25
-0.25 ]
t = 4.8s; x = [ 0.879204 0.691593 2.45 -4.020796 0.25 -0.25 0.25
-0.25 ]
t = 4.9s; x = [ 0.904204 0.666593 2.475 -4.045796 0.25 -0.25 0.25
-0.25 ]
t = 5.0s; x = [ 0.929204 0.641593 2.5 -4.070796 0.25 -0.25 0.25
-0.25 ]
t = 5.1s; x = [ 0.954204 0.616593 2.525 -4.095796 0.25 -0.25 0.25
-0.25 ]
t = 5.2s; x = [ 0.979204 0.591593 2.55 -4.120796 0.25 -0.25 0.25
-0.25 ]
t = 5.3s; x = [ 1.004204 0.566593 2.575 -4.145796 0.25 -0.25 0.25
-0.25 ]
t = 5.4s; x = [ 1.029204 0.541593 2.6 -4.170796 0.25 -0.25 0.25
-0.25 ]
t = 5.5s; x = [ 1.054204 0.516593 2.625 -4.195796 0.25 -0.25 0.25
-0.25 ]
t = 5.6s; x = [ 1.079204 0.491593 2.65 -4.220796 0.25 -0.25 0.25
-0.25 ]
t = 5.7s; x = [ 1.104204 0.466593 2.675 -4.245796 0.25 -0.25 0.25
-0.25 ]
t = 5.8s; x = [ 1.129204 0.441593 2.7 -4.270796 0.25 -0.25 0.25
-0.25 ]
t = 5.9s; x = [ 1.154204 0.416593 2.725 -4.295796 0.25 -0.25 0.25
-0.25 ]
t = 6.0s; x = [ 1.179204 0.391593 2.75 -4.320796 0.25 -0.25 0.25
-0.25 ]
t = 6.1s; x = [ 1.204204 0.366593 2.775 -4.345796 0.25 -0.25 0.25
-0.25 ]
t = 6.2s; x = [ 1.229204 0.341593 2.8 -4.370796 0.25 -0.25 0.25
-0.25 ]
t = 6.3s; x = [ 1.254204 0.316593 2.825 -4.395796 0.25 -0.25 0.25
-0.25 ]
t = 6.4s; x = [ 1.279204 0.291593 2.85 -4.420796 0.25 -0.25 0.25
-0.25 ]
t = 6.5s; x = [ 1.304204 0.266593 2.875 -4.445796 0.25 -0.25 0.25
-0.25 ]
t = 6.6s; x = [ 1.329204 0.241593 2.9 -4.470796 0.25 -0.25 0.25
-0.25 ]
t = 6.7s; x = [ 1.354204 0.216593 2.925 -4.495796 0.25 -0.25 0.25
-0.25 ]
t = 6.8s; x = [ 1.379204 0.191593 2.95 -4.520796 0.25 -0.25 0.25
-0.25 ]
t = 6.9s; x = [ 1.404204 0.166593 2.975 -4.545796 0.25 -0.25 0.25
-0.25 ]
t = 7.0s; x = [ 1.429204 0.141593 3. -4.570796 0.25 -0.25 0.25
-0.25 ]
t = 7.1s; x = [ 1.454204 0.116593 3.025 -4.595796 0.25 -0.25 0.25
-0.25 ]
t = 7.2s; x = [ 1.479204 0.091593 3.05 -4.620796 0.25 -0.25 0.25
-0.25 ]
t = 7.3s; x = [ 1.504204 0.066593 3.075 -4.645796 0.25 -0.25 0.25
-0.25 ]
t = 7.4s; x = [ 1.529204 0.041593 3.1 -4.670796 0.25 -0.25 0.25
-0.25 ]
t = 7.5s; x = [ 1.554204 0.016593 3.125 -4.695796 0.25 -0.25 0.25
-0.25 ]
t = 7.6s; x = [ 1.579204 -0.008407 3.15 -4.720796 0.249997 -0.249993 0.249997
-0.250002]
t = 7.7s; x = [ 1.604203 -0.033407 3.175 -4.745797 0.25 -0.25 0.25
-0.25 ]
t = 7.8s; x = [ 1.629203 -0.058406 3.199999 -4.770797 0.250005 -0.250013 0.250005
-0.249997]
t = 7.9s; x = [ 1.654203 -0.083406 3.224999 -4.795797 0.250005 -0.250013 0.250005
-0.249997]
t = 8.0s; x = [ 1.679203 -0.108406 3.25 -4.820797 0.250004 -0.250009 0.250004
-0.249998]
t = 8.1s; x = [ 1.704204 -0.133407 3.275 -4.845796 0.250003 -0.250007 0.250003
-0.249998]
t = 8.2s; x = [ 1.729204 -0.158408 3.3 -4.870796 0.250002 -0.250005 0.250002
-0.249999]
t = 8.3s; x = [ 1.754204 -0.183408 3.325 -4.895796 0.250001 -0.250004 0.250001
-0.249999]
t = 8.4s; x = [ 1.779204 -0.208408 3.35 -4.920796 0.250001 -0.250003 0.250001
-0.249999]
t = 8.5s; x = [ 1.804204 -0.233408 3.375 -4.945796 0.250001 -0.250003 0.250001
-0.249999]
t = 8.6s; x = [ 1.829204 -0.258409 3.400001 -4.970796 0.250001 -0.250002 0.250001
-0.25 ]
t = 8.7s; x = [ 1.854204 -0.283409 3.425001 -4.995796 0.250001 -0.250002 0.250001
-0.25 ]
t = 8.8s; x = [ 1.879204 -0.308409 3.450001 -5.020796 0.250001 -0.250002 0.250001
-0.25 ]
t = 8.9s; x = [ 1.904204 -0.333409 3.475001 -5.045796 0.250001 -0.250001 0.250001
-0.25 ]
t = 9.0s; x = [ 1.929204 -0.358409 3.500001 -5.070796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.1s; x = [ 1.954204 -0.383409 3.525001 -5.095796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.2s; x = [ 1.979204 -0.40841 3.550001 -5.120796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.3s; x = [ 2.004205 -0.43341 3.575001 -5.145796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.4s; x = [ 2.029205 -0.45841 3.600001 -5.170796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.5s; x = [ 2.054205 -0.48341 3.625001 -5.195796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.6s; x = [ 2.079205 -0.50841 3.650001 -5.220796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.7s; x = [ 2.104205 -0.53341 3.675001 -5.245796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.8s; x = [ 2.129205 -0.55841 3.700001 -5.270796 0.25 -0.250001 0.250001
-0.25 ]
t = 9.9s; x = [ 2.154205 -0.58341 3.725001 -5.295796 0.25 -0.250001 0.250001
-0.25 ]
t = 10.0s; x = [ 2.179205 -0.60841 3.750001 -5.320796 0.25 -0.250001 0.250001
-0.25 ]
<SpStatusEnum.REACHED_END_TIME: 1>
Python script#
Let’s use the simulation we already have to create a Python script. This will dump just the simulation information.
tf = NamedTemporaryFile(suffix=".py")
sim.toDS().toFile(tf.name)
[warning] Model auto_time_display (TimeDisplay) does not have a "toDS" method, so it cannot be added to the StatePropagatorDS.
[warning] Model data_plotter (DataPlotter) does not have a "toDS" method, so it cannot be added to the StatePropagatorDS.
1 file reformatted
This call will show the file in the Jupyter notebook
from IPython.display import Markdown
from pathlib import Path
Markdown(f"```python\n{Path(tf.name).read_text()}\n```")
# This is an auto-generated file for the Sim
# This file was generated on 2026-06-30 10:02:56.207202
import atexit
import Karana.Dynamics as kd
import Karana.Models as kmdl
from typing import cast
from Karana.Math.Kquantities import ureg
import Karana.Scene as ks
import numpy as np
import Karana.Integrators as ki
from Karana.KUtils.Sim import Sim
import Karana.Math as km
def populateMultibodyAndStatePropagator(mb: kd.Multibody, sp: kd.StatePropagator):
"""Populate and configure the provided Multibody and StatePropagator.
Parameters
----------
mb : kd.Multibody
The Multibody to populate with physical bodies.
sp : kd.StatePropagator
The StatePropagator to populate and configure.
"""
# This creates, adds, and connects physical bodies via hinges,
# also referred to as joints, to the provided Multibody instance.
# Often, the input `Multibody' instance is empty and contains just a
# virtual root body.
#
# The code below consists of repeated blocks - one per body. Each block
# defines the mass properties for the body, the parent hinge type and
# location on the body and its parent body, and the mesh geometries attached.
# ----------------------------------------------------------------------------------------------
# Look up the unique virtual root body for the multibody system that
# will serve as an anchor for the new physical bodies.
vroot = mb.virtualRoot()
# ----------------------------------------------------------------------------------------------
# Create 'bd1' body
# ----------------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------
# Start defining meshes for 'bd1' body"
# Create one or more meshes attached to the 'bd1' body. Each mesh
# consists of the shape and color/texture definition for its visual appearance.
# The shapes can be parameterized shapes (e.g., spheres, boxes, etc., or be
# defined in external files such as '.glb', '.obj', etc. formats.
# ----------------------------------------------------------------------------------------------
# Define the 'bd1_beginning' mesh parameters for 'bd1' body
# Define the part shape (geometry)
cylinder = ks.CylinderGeometry(0.10000000149011612, 0.10000000149011612)
# Define the part color/texture material
physical_material_info = ks.PhysicalMaterialInfo()
physical_material_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_info.roughness = 1.0
physical_material = ks.PhysicalMaterial(physical_material_info)
bd1_beginning = ks.ScenePartSpec(
name="bd1_beginning",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.5]) * ureg.meter,
),
geometry=cylinder,
material=physical_material,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# ----------------------------------------------------------------------------------------------
# Define the 'bd1_body' mesh parameters for 'bd1' body
# Define the part shape (geometry)
box = ks.BoxGeometry(0.05000000074505806, 0.05000000074505806, 1.0)
# Define the part color/texture material
physical_material_1_info = ks.PhysicalMaterialInfo()
physical_material_1_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_1_info.roughness = 1.0
physical_material_1 = ks.PhysicalMaterial(physical_material_1_info)
bd1_body = ks.ScenePartSpec(
name="bd1_body",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.0]) * ureg.meter,
),
geometry=box,
material=physical_material_1,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# ----------------------------------------------------------------------------------------------
# Define the 'bd1_pivot' mesh parameters for 'bd1' body
# Define the part shape (geometry)
cylinder_1 = ks.CylinderGeometry(0.10000000149011612, 0.10000000149011612)
# Define the part color/texture material
physical_material_2_info = ks.PhysicalMaterialInfo()
physical_material_2_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_2_info.roughness = 1.0
physical_material_2 = ks.PhysicalMaterial(physical_material_2_info)
bd1_pivot = ks.ScenePartSpec(
name="bd1_pivot",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, -0.5]) * ureg.meter,
),
geometry=cylinder_1,
material=physical_material_2,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# End defining meshes for 'bd1' body
# ----------------------------------------------------------------------------------------------
# Assemble overall parameters needed to define the 'bd1' body
# including mass and kinematics properties.
bd1_params = kd.PhysicalBodyParams(
# 'bd1' mass properties
spI=km.SpatialInertia(
2.0 * ureg.kilogram,
np.array([0.0, 0.0, 0.0]) * ureg.meter,
np.array([[3.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 1.0]])
* ureg.kilogram
* ureg.meter**2,
),
# Define the location and orientation of the 'bd1' parent hinge
# relative to the 'bd1' body frame
body_to_joint_transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([-0.0, -0.0, 0.5]) * ureg.meter,
),
# Define the location and orientation of the 'bd1' parent hinge
# relative to the parent body's frame
inb_to_joint_transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([1.0, 0.0, 0.0]) * ureg.meter,
),
# define the 'bd1' parent hinge type and parameters
hinge_params=kd.PhysicalHingeParams(
hinge_type=kd.HingeType.REVOLUTE,
subhinge_params=[
kd.PinSubhingeParams(
unit_axis=np.array([0.0, 1.0, 0.0]),
prescribed=False,
),
],
),
# Add 'bd1' scene parts defined above
scene_part_specs=[bd1_beginning, bd1_body, bd1_pivot],
)
# Create the 'bd1' body
bd1 = kd.PhysicalBody("bd1", mb)
# Connect the 'bd1' body to its parent via a hinge
kd.PhysicalHinge(vroot, bd1, kd.HingeType.REVOLUTE)
# Set the parameters for the 'bd1' body
bd1.setParams(bd1_params)
# ----------------------------------------------------------------------------------------------
# Create 'bd2' body
# ----------------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------
# Start defining meshes for 'bd2' body"
# Create one or more meshes attached to the 'bd2' body. Each mesh
# consists of the shape and color/texture definition for its visual appearance.
# The shapes can be parameterized shapes (e.g., spheres, boxes, etc., or be
# defined in external files such as '.glb', '.obj', etc. formats.
# ----------------------------------------------------------------------------------------------
# Define the 'bd1_pivot' mesh parameters for 'bd2' body
# Define the part shape (geometry)
cylinder_2 = ks.CylinderGeometry(0.10000000149011612, 0.10000000149011612)
# Define the part color/texture material
physical_material_3_info = ks.PhysicalMaterialInfo()
physical_material_3_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_3_info.roughness = 1.0
physical_material_3 = ks.PhysicalMaterial(physical_material_3_info)
bd1_pivot_1 = ks.ScenePartSpec(
name="bd1_pivot",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.0]) * ureg.meter,
),
geometry=cylinder_2,
material=physical_material_3,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# ----------------------------------------------------------------------------------------------
# Define the 'bd2_body' mesh parameters for 'bd2' body
# Define the part shape (geometry)
box_1 = ks.BoxGeometry(0.05000000074505806, 0.05000000074505806, 1.0)
# Define the part color/texture material
physical_material_4_info = ks.PhysicalMaterialInfo()
physical_material_4_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_4_info.roughness = 1.0
physical_material_4 = ks.PhysicalMaterial(physical_material_4_info)
bd2_body = ks.ScenePartSpec(
name="bd2_body",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.5]) * ureg.meter,
),
geometry=box_1,
material=physical_material_4,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# End defining meshes for 'bd2' body
# ----------------------------------------------------------------------------------------------
# Assemble overall parameters needed to define the 'bd2' body
# including mass and kinematics properties.
bd2_params = kd.PhysicalBodyParams(
# 'bd2' mass properties
spI=km.SpatialInertia(
2.0 * ureg.kilogram,
np.array([0.0, 0.0, 0.0]) * ureg.meter,
np.array([[3.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 1.0]])
* ureg.kilogram
* ureg.meter**2,
),
# Define the location and orientation of the 'bd2' parent hinge
# relative to the 'bd2' body frame
body_to_joint_transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([-0.0, -0.0, 1.0]) * ureg.meter,
),
# Define the location and orientation of the 'bd2' parent hinge
# relative to the parent body's frame
inb_to_joint_transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, -0.5]) * ureg.meter,
),
# define the 'bd2' parent hinge type and parameters
hinge_params=kd.PhysicalHingeParams(
hinge_type=kd.HingeType.REVOLUTE,
subhinge_params=[
kd.PinSubhingeParams(
unit_axis=np.array([0.0, 1.0, 0.0]),
prescribed=False,
),
],
),
# Add 'bd2' scene parts defined above
scene_part_specs=[bd1_pivot_1, bd2_body],
)
# Create the 'bd2' body
bd2 = kd.PhysicalBody("bd2", mb)
# Connect the 'bd2' body to its parent via a hinge
kd.PhysicalHinge(bd1, bd2, kd.HingeType.REVOLUTE)
# Set the parameters for the 'bd2' body
bd2.setParams(bd2_params)
# ----------------------------------------------------------------------------------------------
# Create 'bd3' body
# ----------------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------
# Start defining meshes for 'bd3' body"
# Create one or more meshes attached to the 'bd3' body. Each mesh
# consists of the shape and color/texture definition for its visual appearance.
# The shapes can be parameterized shapes (e.g., spheres, boxes, etc., or be
# defined in external files such as '.glb', '.obj', etc. formats.
# ----------------------------------------------------------------------------------------------
# Define the 'bd1_pivot' mesh parameters for 'bd3' body
# Define the part shape (geometry)
cylinder_3 = ks.CylinderGeometry(0.10000000149011612, 0.10000000149011612)
# Define the part color/texture material
physical_material_5_info = ks.PhysicalMaterialInfo()
physical_material_5_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_5_info.roughness = 1.0
physical_material_5 = ks.PhysicalMaterial(physical_material_5_info)
bd1_pivot_2 = ks.ScenePartSpec(
name="bd1_pivot",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.0]) * ureg.meter,
),
geometry=cylinder_3,
material=physical_material_5,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# ----------------------------------------------------------------------------------------------
# Define the 'bd2_body' mesh parameters for 'bd3' body
# Define the part shape (geometry)
box_2 = ks.BoxGeometry(0.05000000074505806, 0.05000000074505806, 1.0)
# Define the part color/texture material
physical_material_6_info = ks.PhysicalMaterialInfo()
physical_material_6_info.color = ks.Color.fromRGBA(
0.6980392336845398, 0.13333334028720856, 0.13333334028720856, 1.0
)
physical_material_6_info.roughness = 1.0
physical_material_6 = ks.PhysicalMaterial(physical_material_6_info)
bd2_body_1 = ks.ScenePartSpec(
name="bd2_body",
transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.5]) * ureg.meter,
),
geometry=box_2,
material=physical_material_6,
scale=np.array([1.0, 1.0, 1.0]),
layers=419430400,
)
# End defining meshes for 'bd3' body
# ----------------------------------------------------------------------------------------------
# Assemble overall parameters needed to define the 'bd3' body
# including mass and kinematics properties.
bd3_params = kd.PhysicalBodyParams(
# 'bd3' mass properties
spI=km.SpatialInertia(
2.0 * ureg.kilogram,
np.array([0.0, 0.0, 0.0]) * ureg.meter,
np.array([[3.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 1.0]])
* ureg.kilogram
* ureg.meter**2,
),
# Define the location and orientation of the 'bd3' parent hinge
# relative to the 'bd3' body frame
body_to_joint_transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([-0.0, -0.0, 1.0]) * ureg.meter,
),
# Define the location and orientation of the 'bd3' parent hinge
# relative to the parent body's frame
inb_to_joint_transform=km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.0]) * ureg.meter,
),
# define the 'bd3' parent hinge type and parameters
hinge_params=kd.PhysicalHingeParams(
hinge_type=kd.HingeType.REVOLUTE,
subhinge_params=[
kd.PinSubhingeParams(
unit_axis=np.array([0.0, 1.0, 0.0]),
prescribed=False,
),
],
),
# Add 'bd3' scene parts defined above
scene_part_specs=[bd1_pivot_2, bd2_body_1],
)
# Create the 'bd3' body
bd3 = kd.PhysicalBody("bd3", mb)
# Connect the 'bd3' body to its parent via a hinge
kd.PhysicalHinge(bd2, bd3, kd.HingeType.REVOLUTE)
# Set the parameters for the 'bd3' body
bd3.setParams(bd3_params)
# Constraint node cn
cn = kd.ConstraintNode.lookupOrCreate("cn", bd3)
cn.setBodyToNodeTransform(
km.HomTran(
km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
np.array([0.0, 0.0, 0.0]) * ureg.meter,
)
)
# constraint kd.LoopConstraintCutJoint
lc = kd.LoopConstraintCutJoint(
"constraint",
mb,
cn.frameToFrame(vroot),
kd.HingeType.REVOLUTE,
)
sh = cast(kd.PinSubhinge, lc.hinge().subhinge(0))
sh.setUnitAxis(np.array([0.0, 1.0, 0.0]))
# ----------------------------------------------------------------------------------------------
# Finished physical body additions
# ----------------------------------------------------------------------------------------------
# Finalize the multibody for the added bodies
mb.ensureHealthy()
# ----------------------------------------------------------------------------------------------
# Populate and configure the StatePropagator. This will involve
# defining the type of solver to use for the multibody equations of
# motion, adding an integrator to use for state propagation,
# addition of KModels for interacting with the multibody dynamics,
# and timed events for execution during time advancement.
# ----------------------------------------------------------------------------------------------
# Set the StatePropagator's solver type. 'FORWARD_DYNAMICS' is the
# typical choice for setting up the propagator to solve the
# equations of motion for time domain simulations
sp.setSolverType(kd.MMSolverType.FORWARD_DYNAMICS)
# ----------------------------------------------------------------------------------------------
# Define integrator options
cvode_opts = ki.CVodeIntegratorOptions()
cvode_opts.rtol = 0.0001
cvode_opts.atol = 1e-08
cvode_opts.max_num_steps = 1000000
cvode_opts.min_step_size = 1e-12
cvode_opts.anderson_damping = 1.0
cvode_opts.anderson_length = 0
cvode_opts.max_nl_iters = 3
# Set up the integrator
sp.setIntegrator(ki.IntegratorType.CVODE, cvode_opts)
# ----------------------------------------------------------------------------------------------
# Set the StatePropagator options
# Call with True to make an extra dynamics derivative call at the
# end of a simulation step to update the acceleration values.
sp.setUpdateStateDerivativesHopEnd(False)
# Set a limit on the maximum integration time step size.
# A value of 0.0 means there is no maximum limit.
sp.setMaxStepSize(0.0)
# ----------------------------------------------------------------------------------------------
# Add KModels. KModels are parameterized models for
# component actuator, sensors, environmental, etc. that interact
# with the dynamics.
# ----------------------------------------------------------------------------------------------
# ----------------------------------------------------------------------------------------------
# Add the sync_real_time KModel
sync_real_time = kmdl.SyncRealTime("sync_real_time", sp, 1.0)
# ----------------------------------------------------------------------------------------------
# Add the update_proxy_scene KModel
multibody_scene = mb.getScene()
if multibody_scene is not None:
update_proxy_scene = kmdl.UpdateProxyScene("update_proxy_scene", sp, multibody_scene)
# ----------------------------------------------------------------------------------------------
# Add the gil_release KModel
gil_release = kmdl.GilRelease("gil_release", sp)
# Finalize the StatePropagator for the updated multibody system, the
# KModels and the integrator
sp.hardReset()
sp.ensureHealthy()
# End setting up StatePropagator and KModels
# ----------------------------------------------------------------------------------------------
if __name__ == "__main__":
# Create a simulation
sim = Sim()
# Populate the Multibody and StatePropagator for the Sim
populateMultibodyAndStatePropagator(sim.mb, sim.sp)
# Code to cleanup upon exit
def cleanup():
global sim
del sim
atexit.register(cleanup)
Above, you’ll see the multibody code similar to the time domain notebook, as well as code to add the Gravity KModel.
del sim
globals().update(runpy.run_path(tf.name, run_name="__main__"))
Let’s bring up the GUI again just to show we captured everything
# Create the GUI
sim.setupGui()
# Update the camera view
sim.mb.resetData()
sim.mb.getScene().viewAroundFrame(
sim.mb.virtualRoot(), np.array([0.0, 5.0, 0.0]), np.zeros(3), np.array([0.0, 0.0, 1.0])
)
# Redraw the scene
sim.mb.getScene().update()
[WebUI] Web server is running on port 43427
You may be able to connect in your browser at:
http://newton:43427
[WebUI] Karana Viewer is running on port 29534
Open this URL to connect:
http://newton:29534
Cleanup#
def cleanup():
"""Clean up the simulation."""
global sim, sp
del sim, sp
atexit.register(cleanup)
<function __main__.cleanup()>