Slider crank example#
This notebook is an example of a simple slider crank. This demonstrates how to do the following:
Requirements:
In this tutorial we will:
Create a Multibody using DataStruct.DataStruct
Use ConstraintKinematicsSolver solve a system
Setup the simulation
Run the simulation with constraints
For a more in-depth descriptions of kdflex concepts see usage.
import numpy as np
import atexit
from copy import deepcopy
from typing import cast
import quantities as pq
import Karana.Core as kc
import Karana.Math as km
import Karana.Frame as kf
import Karana.Dynamics as kd
import Karana.Dynamics.SOADyn_types as kdt
import Karana.Scene as ks
import Karana.Models as kmdl
Create a Multibody Using DataStruct#
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.
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.
fc = kf.FrameContainer("root")
# Create a BodyDS for the first link. This can be used to specify all the parameters for the body, and the way
# the body attaches to it's parent, i.e., the hinge.
link_1 = kdt.BodyDS(
name="link_1",
mass=1.0 * pq.lb,
body_to_cm=np.zeros(3),
inertia=np.eye(3),
hinge=kdt.HingeDS(
hinge_type=kd.HingeType.REVOLUTE,
subhinges=[
kdt.PinSubhingeDS(prescribed=False, unit_axis=np.array([0.0, 1.0, 0.0]))
], # pyright: ignore - Types are okay
),
body_to_joint=np.array([-0.25, 0.0, 0.0]),
)
# Let's create the second link of our slider crank. Here, we will simply copy the first and modify the parameters
# that should be changed.
link_2 = deepcopy(link_1)
link_2.name = "link_2"
link_2.mass = 2.0 * pq.kg
link_2.body_to_joint = np.array([0.75, 0.0, 0.0])
link_2.inb_to_joint = np.array([0.25, 0.0, 0.0])
# We create a block body in a similar fashion. The block is the body that will have the slider constraint,
# so we also add a constraint node via a NodeDS.
block = deepcopy(link_1)
block.name = "block"
block.mass = 1.0
block.body_to_joint = np.zeros(3)
block.constraint_nodes = [
kdt.NodeDS(
name="cn",
constraint_node=True,
force_node=True,
)
]
# Now, we combine all the bodies together to form a SubGraphDS. In addition, we add our LoopConstraint
# and a constraint frame for the slider constraint.
mbody_ds = kdt.SubGraphDS(
name="mb",
base_bodies=[
kdt.BodyWithContextDS(
body=link_1,
children=[
kdt.BodyWithContextDS(
body=link_2,
children=[
kdt.BodyWithContextDS(body=block, children=[]),
],
),
],
)
],
loop_constraints=[
kdt.LoopConstraintCutJointDS(
name="block_constraint",
hinge=kdt.HingeDS(
hinge_type=kd.HingeType.SLIDER,
subhinges=[
kdt.LinearSubhingeDS(prescribed=False, unit_axis=np.array([1.0, 0.0, 0.0]))
],
),
get_oframe={"frame": "constraint_frame"},
get_pframe={"body": "block", "node": "cn"},
)
],
constraint_frames=[
kdt.ConstraintFrameDS(
name="constraint_frame",
translation=np.zeros(3),
unit_quaternion=km.UnitQuaternion(0, 0, 0, 1),
parent="newtonian",
)
],
)
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.
Here, we don’t need to save the DataStruct for later, we simply use it to create the Multibody.
# Create the Multibody from the DataStruct
mb = mbody_ds.toMultibody(fc)
del mbody_ds, link_2, block
# Once all bodies are created, we make the Multibody healthy. This sets up internal
# variables for use in dynamics computations.
mb.ensureHealthy()
As mentioned previously, values are automatically converted to the correct units system.
print(f"DataStruct value for link_1 mass: {link_1.mass}")
link_1_bd = cast(kd.PhysicalBody, mb.getBody("link_1"))
print(f"Mass for link_1 in the multibody: {link_1_bd.getSpatialInertia().mass()} kg")
del link_1_bd, link_1
DataStruct value for link_1 mass: 1.0 lb
Mass for link_1 in the multibody: 0.45359237 kg
Use ConstraintKinematicsSolver to Solve the System#
Now, we initialize the state with the Karana.Dynamics.ConstraintKinematicsSolver
# Now, we need to initialize our state. Our state has constraints, so we will use the
# ConstraintKinematicsSolver to get a valid state that satisfies the constraints.
# First, we set everything to zero as our initial guess, except the velocity of the
# first link. We want this to have an initial velocity so that our slider-crank moves.
cd = mb.subhingeCoordData()
cd.setQ(0.0)
cd.setU(0.0)
u = cd.getU()
u[0] = 1.0
cd.setU(u)
cd.setUdot(0.0)
cd.setT(0.0)
# In addition to the subhinge data, we also need to set the constraint data. We do so here.
cdc = mb.cutjointCoordData()
cdc.setQ(0.0)
cdc.setU(0.0)
cdc.setUdot(0.0)
cdc.setT(0.0)
del cdc
The 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.
# Now, we create a ConstraintKinematicsSolver and solve for the initial state.
cks = mb.cks()
errQ = cks.solveQ()
errU = cks.solveU()
# Make sure we were able to solve for Q and U
assert abs(errQ) < 1e-6
assert abs(errU) < 1e-6
del cks
# Before we finalize everything (see below), we need to set the gravity. Ultimately, we will
# be setting this with a model (see further below). For now, we will just set this to zero
# so we can do the readiness check.
mb.setUniformGravAccel(np.zeros(3))
# verify the multibody and its state
assert kc.allReady()
Setup the Simulation#
Start by calling the setupGraphics helper method on the multibody. This method takes care of setting up the graphics environment.
See Visualization and Scene Layer for more information relating to this section.
# setup graphics helper function
cleanup_graphics, web_scene = mb.setupGraphics(port=0, axes=0.5)
mb.createStickParts()
# position the viewpoint camera of the visualization
web_scene.defaultCamera().pointCameraAt([0, 3, 0], [0, 0, 0], [0, 0, 1])
del web_scene
Setup the Karana.Dynamics.StatePropagator and integrator.
# Set up state propagator and select integrator type: rk4 or cvode
sp = kd.StatePropagator(
mb,
integrator_type=km.IntegratorType.RK4,
integ_opts=None,
solver_type=kd.MMSolverType.TREE_AUGMENTED_DYNAMICS,
)
integrator = sp.getIntegrator()
# Makes sure the visualization scene is updated after each state change.
kmdl.UpdateProxyScene("update_proxy_scene", sp, mb.getScene())
# sync the simulation time with real-time.
kmdl.SyncRealTime("sync_real_time", sp, 1.0)
# register callback to print the state at each step
f2f = fc.root().frameToFrame(mb.getBody("block"))
def post_step_fn(t, x):
"""Print out the time and state.
Parameters
----------
t : float
The current time.
x : NDArray[np.float64]
The current state.
"""
print(
f"t = {float(integrator.getTime())/1e9}s; block_pos = {f2f.relTransform().getTranslation()}"
)
sp.fns.post_hop_fns["update_and_info"] = post_step_fn
Initialize integrator state and add timed event
# Initialize integrator state
t_init = np.timedelta64(0, "ns")
sp.setTime(t_init)
sp.setState(sp.assembleState())
# Syncs up graphics
mb.getScene().update()
# register the timed event
h = np.timedelta64(int(1e8), "ns")
t = kd.TimedEvent("hop_size", h, lambda _: None, False)
t.period = h
sp.registerTimedEvent(t)
del t
Run the Simulation With Constraints#
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.
# run the simulation
print(f"t = {float(integrator.getTime())/1e9}s; x = {integrator.getX()}")
# run the simulation
sp.advanceTo(10.0)
# dump the state propagator info
sp.dump("sp")
t = 0.0s; x = [ 0. 0. 0. -0.25 1. -0.33333333
-0.66666667 0. ]
t = 0.1s; block_pos = [-2.50835263e-01 0.00000000e+00 2.66120017e-08]
t = 0.2s; block_pos = [-2.53364350e-01 0.00000000e+00 5.41549326e-08]
t = 0.3s; block_pos = [-2.57658048e-01 0.00000000e+00 8.36652793e-08]
t = 0.4s; block_pos = [-2.63837196e-01 0.00000000e+00 1.15819022e-07]
t = 0.5s; block_pos = [-2.72076493e-01 0.00000000e+00 1.50486238e-07]
t = 0.6s; block_pos = [-2.82608535e-01 0.00000000e+00 1.85912595e-07]
t = 0.7s; block_pos = [-2.95725945e-01 0.00000000e+00 2.17486330e-07]
t = 0.8s; block_pos = [-3.11777550e-01 0.00000000e+00 2.36557962e-07]
t = 0.9s; block_pos = [-3.31151907e-01 0.00000000e+00 2.31129365e-07]
t = 1.0s; block_pos = [-3.54239340e-01 0.00000000e+00 1.92102924e-07]
t = 1.1s; block_pos = [-3.81365729e-01 0.00000000e+00 1.27370642e-07]
t = 1.2s; block_pos = [-4.12703553e-01 0.00000000e+00 7.38739071e-08]
t = 1.3s; block_pos = [-4.48188762e-01 0.00000000e+00 8.23730407e-08]
t = 1.4s; block_pos = [-4.87488836e-01 0.00000000e+00 1.72212803e-07]
t = 1.5s; block_pos = [-5.30049758e-01 0.00000000e+00 3.09150500e-07]
t = 1.6s; block_pos = [-5.75200703e-01 0.00000000e+00 4.39428533e-07]
t = 1.7s; block_pos = [-6.22262221e-01 0.00000000e+00 5.32065287e-07]
t = 1.8s; block_pos = [-6.70616564e-01 0.00000000e+00 5.85742556e-07]
t = 1.9s; block_pos = [-7.19733069e-01 0.00000000e+00 6.13435375e-07]
t = 2.0s; block_pos = [-7.69162683e-01 0.00000000e+00 6.28946852e-07]
t = 2.1s; block_pos = [-8.18518032e-01 0.00000000e+00 6.41799976e-07]
t = 2.2s; block_pos = [-8.67449397e-01 0.00000000e+00 6.57142545e-07]
t = 2.3s; block_pos = [-9.15620994e-01 0.00000000e+00 6.77132508e-07]
t = 2.4s; block_pos = [-9.62688431e-01 0.00000000e+00 7.02232464e-07]
t = 2.5s; block_pos = [-1.00827661e+00 0.00000000e+00 7.32054536e-07]
t = 2.6s; block_pos = [-1.05195695e+00 0.00000000e+00 7.65833100e-07]
t = 2.7s; block_pos = [-1.09322319e+00 0.00000000e+00 8.02703910e-07]
t = 2.8s; block_pos = [-1.13146638e+00 0.00000000e+00 8.42000555e-07]
t = 2.9s; block_pos = [-1.16595277e+00 0.00000000e+00 8.83831970e-07]
t = 3.0s; block_pos = [-1.19581384e+00 0.00000000e+00 9.30178900e-07]
t = 3.1s; block_pos = [-1.22006652e+00 0.00000000e+00 9.86196835e-07]
t = 3.2s; block_pos = [-1.23768881e+00 0.00000000e+00 1.05974510e-06]
t = 3.3s; block_pos = [-1.24776842e+00 0.00000000e+00 1.15584883e-06]
t = 3.4s; block_pos = [-1.24970291e+00 0.00000000e+00 1.26785947e-06]
t = 3.5s; block_pos = [-1.24336897e+00 0.00000000e+00 1.37749156e-06]
t = 3.6s; block_pos = [-1.22916088e+00 0.00000000e+00 1.46895060e-06]
t = 3.7s; block_pos = [-1.20787213e+00 0.00000000e+00 1.53971250e-06]
t = 3.8s; block_pos = [-1.18049646e+00 0.00000000e+00 1.59631597e-06]
t = 3.9s; block_pos = [-1.14804876e+00 0.00000000e+00 1.64571041e-06]
t = 4.0s; block_pos = [-1.11145619e+00 0.00000000e+00 1.69167256e-06]
t = 4.1s; block_pos = [-1.07151534e+00 0.00000000e+00 1.73531760e-06]
t = 4.2s; block_pos = [-1.02889097e+00 0.00000000e+00 1.77644273e-06]
t = 4.3s; block_pos = [-9.84134123e-01 0.00000000e+00 1.81440708e-06]
t = 4.4s; block_pos = [-9.37706458e-01 0.00000000e+00 1.84854887e-06]
t = 4.5s; block_pos = [-8.90005078e-01 0.00000000e+00 1.87842145e-06]
t = 4.6s; block_pos = [-8.41385914e-01 0.00000000e+00 1.90408227e-06]
t = 4.7s; block_pos = [-7.92186007e-01 0.00000000e+00 1.92661065e-06]
t = 4.8s; block_pos = [-7.42745712e-01 0.00000000e+00 1.94898329e-06]
t = 4.9s; block_pos = [-6.93431844e-01 0.00000000e+00 1.97726863e-06]
t = 5.0s; block_pos = [-6.44661829e-01 0.00000000e+00 2.02151789e-06]
t = 5.1s; block_pos = [-5.96926428e-01 0.00000000e+00 2.09438169e-06]
t = 5.2s; block_pos = [-5.50803674e-01 0.00000000e+00 2.20393968e-06]
t = 5.3s; block_pos = [-5.06950102e-01 0.00000000e+00 2.33951657e-06]
t = 5.4s; block_pos = [-4.66052276e-01 0.00000000e+00 2.46182169e-06]
t = 5.5s; block_pos = [-4.28733184e-01 0.00000000e+00 2.52148124e-06]
t = 5.6s; block_pos = [-3.95438954e-01 0.00000000e+00 2.50352447e-06]
t = 5.7s; block_pos = [-3.66359362e-01 0.00000000e+00 2.44386597e-06]
t = 5.8s; block_pos = [-3.41423812e-01 0.00000000e+00 2.39251089e-06]
t = 5.9s; block_pos = [-3.20366214e-01 0.00000000e+00 2.37500228e-06]
t = 6.0s; block_pos = [-3.02816295e-01 0.00000000e+00 2.38976466e-06]
t = 6.1s; block_pos = [-2.88378318e-01 0.00000000e+00 2.42412620e-06]
t = 6.2s; block_pos = [-2.76681813e-01 0.00000000e+00 2.46629842e-06]
t = 6.3s; block_pos = [-2.67406922e-01 0.00000000e+00 2.50905808e-06]
t = 6.4s; block_pos = [-2.60293115e-01 0.00000000e+00 2.54915934e-06]
t = 6.5s; block_pos = [-2.55139177e-01 0.00000000e+00 2.58580872e-06]
t = 6.6s; block_pos = [-2.51799701e-01 0.00000000e+00 2.61946151e-06]
t = 6.7s; block_pos = [-2.50180994e-01 0.00000000e+00 2.65111438e-06]
t = 6.8s; block_pos = [-2.50237855e-01 0.00000000e+00 2.68195172e-06]
t = 6.9s; block_pos = [-2.51971872e-01 0.00000000e+00 2.71317988e-06]
t = 7.0s; block_pos = [-2.55431489e-01 0.00000000e+00 2.74592070e-06]
t = 7.1s; block_pos = [-2.60713815e-01 0.00000000e+00 2.78105624e-06]
t = 7.2s; block_pos = [-2.67967937e-01 0.00000000e+00 2.81890799e-06]
t = 7.3s; block_pos = [-2.77399033e-01 0.00000000e+00 2.85860861e-06]
t = 7.4s; block_pos = [-2.89271757e-01 0.00000000e+00 2.89704924e-06]
t = 7.5s; block_pos = [-3.03909846e-01 0.00000000e+00 2.92755471e-06]
t = 7.6s; block_pos = [-3.21686555e-01 0.00000000e+00 2.93934978e-06]
t = 7.7s; block_pos = [-3.42997834e-01 0.00000000e+00 2.92068698e-06]
t = 7.8s; block_pos = [-3.68209642e-01 0.00000000e+00 2.86945263e-06]
t = 7.9s; block_pos = [-3.97577588e-01 0.00000000e+00 2.80866181e-06]
t = 8.0s; block_pos = [-4.31155962e-01 0.00000000e+00 2.78709020e-06]
t = 8.1s; block_pos = [-4.68736392e-01 0.00000000e+00 2.84451672e-06]
t = 8.2s; block_pos = [-5.09857422e-01 0.00000000e+00 2.97052659e-06]
t = 8.3s; block_pos = [-5.53888065e-01 0.00000000e+00 3.11402288e-06]
t = 8.4s; block_pos = [-6.00141274e-01 0.00000000e+00 3.23016801e-06]
t = 8.5s; block_pos = [-6.47964860e-01 0.00000000e+00 3.30509799e-06]
t = 8.6s; block_pos = [-6.9678682e-01 0.0000000e+00 3.3472802e-06]
t = 8.7s; block_pos = [-7.46122007e-01 0.00000000e+00 3.37124283e-06]
t = 8.8s; block_pos = [-7.95557218e-01 0.00000000e+00 3.38871488e-06]
t = 8.9s; block_pos = [-8.44728210e-01 0.00000000e+00 3.40674236e-06]
t = 9.0s; block_pos = [-8.93295579e-01 0.00000000e+00 3.42869198e-06]
t = 9.1s; block_pos = [-9.40921696e-01 0.00000000e+00 3.45568294e-06]
t = 9.2s; block_pos = [-9.87248538e-01 0.00000000e+00 3.48764580e-06]
t = 9.3s; block_pos = [-1.03187536e+00 0.00000000e+00 3.52394889e-06]
t = 9.4s; block_pos = [-1.07433518e+00 0.00000000e+00 3.56374347e-06]
t = 9.5s; block_pos = [-1.11406996e+00 0.00000000e+00 3.60622304e-06]
t = 9.6s; block_pos = [-1.15040639e+00 0.00000000e+00 3.65103377e-06]
t = 9.7s; block_pos = [-1.18253856e+00 0.00000000e+00 3.69911405e-06]
t = 9.8s; block_pos = [-1.20953137e+00 0.00000000e+00 3.75401755e-06]
t = 9.9s; block_pos = [-1.23036702e+00 0.00000000e+00 3.82265815e-06]
t = 10.0s; block_pos = [-1.24405911e+00 0.00000000e+00 3.91245475e-06]
Below, we cleanup everything whenever the script exits. This requires us to delete local variables and discard our containers and scene.
def cleanup():
"""Cleanup the simulation."""
global integrator, cd, f2f
del integrator, cd, f2f
kc.discard(sp)
cleanup_graphics()
mb.ensureNotHealthy()
kc.discard(mb.enabledConstraints())
kc.discard(fc.lookupFrame("constraint_frame"))
kc.discard(mb)
kc.discard(fc)
atexit.register(cleanup)
<function __main__.cleanup()>
Summary#
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.
Further Readings#
Load a mars 2020 rover urdf
Load a robotic arm urdf
Enforce loop constraints in a double-wishbone model
Drive an ATRVjr rover