Closed-loop assignment

Closed-loop assignment#

This notebook servers as a simple assignment (with answers) to help you understand how to build a closed-loop kdFlex simulation.

This notebook assumes that you’ve already gone through the 4-bar notebook and all subsequent notebooks.

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 atexit
import Karana.Math as km
from Karana.Math.Kquantities import ureg
from pathlib import Path

We’ll begin by loading up a 3-link pendulum system.

sim_ds_path = Path("..") / "resources" / "3_link_pendulum" / "3_link_pendulum.h5"
sim = SimDS.fromFile(sim_ds_path).toSim()
sim.mb.resetData()
sim.mb.getScene().viewAroundFrame(
    sim.mb.virtualRoot(), np.array([0.0, 8.0, 0.0]), np.zeros(3), np.array([0.0, 0.0, 1.0])
)
sim.mb.getScene().update()

[WebUI] Web server is running on port 41059
You may be able to connect in your browser at:
	http://newton:41059
[WebUI] Karana Viewer is running on port 29534
Open this URL to connect:
	http://newton:29534
Click to open Karana Viewer in a new tab

Your assignment#

The assignment is to do the following:

  1. clone this system so that there are two 3-link pendulum systems.

  2. Create a constraint between the bottom pendulum bobs of both systems to form a loop. In other words, the two yellow pendulum bobs should be connected via a constraint.

  3. Set an initial state for the system that satisfies the constraints.

  4. Add a Gravity KModel. Use uniform gravity with an acceleration of 9.81 m/s^2 in the negative z-direction.

  5. Simulate the system for 10 seconds.

Assignment answer#

All cells below this one walk through the answer to this assignment. You should try this assignment on your own before looking at these, and only use the cells below if you need some help along the way and/or to confirm your answers.

Cloning the 3-bar system#

There are many ways to clone the 3-bar system. Here, we will use a DataStruct to do so.

# Create a DataStruct of the current system
sg_ds = sim.mb.toDS()

# Now, let's rename the bodies to clearly differential them from the old system
sg_ds.renameBodies("pendulum_2_")

# We will move the attachment to the root 2 meters in the +x direction
sg_ds.base_bodies[0].body.inb_to_joint = np.array([2.0, 0.0, 0.0])

# Finally, let's add these cloned bodies to the multibody
new_sg = sg_ds.toSubGraph(sim.mb, sim.mb.virtualRoot())

Let’s take a look at our new system. We’ll set all coordinates to 0, and then update the scene to see it in the GUI above.

sim.mb.resetData()
sim.mb.getScene().update()

Adding a constraint#

Now, we will add a constraint between the bottom pendulum bobs.

p1_bd3 = sim.mb.getBody("bd3")
p2_bd3 = sim.mb.getBody("pendulum_2_bd3")

cn1 = kd.ConstraintNode.lookupOrCreate("cn_1", p1_bd3)
cn1.setBodyToNodeTransform(km.HomTran())
cn2 = kd.ConstraintNode.lookupOrCreate("cn_2", p2_bd3)
cn2.setBodyToNodeTransform(km.HomTran())

lc = kd.LoopConstraintCutJoint("constraint", sim.mb, cn1.frameToFrame(cn2), kd.HingeType.REVOLUTE)
lc.hinge().subhinge(0).setUnitAxis(np.array([0.0, 1.0, 0.0]))
lc.hinge().subhinge(0).setQ(0.0)
lc.hinge().subhinge(0).setU(0.0)

Initial state#

Here, we will set the initial state of the system. Let’s give both top pendulums an initial position of 0.2 radians in opposite directions along with a velocity of 0.2 radians in opposite directions.

Then, we will use the ConstraintKinematicsSolver to solve for a pose that satisfies the constraints.

bd1 = sim.mb.getBody("bd1")
p2_bd1 = sim.mb.getBody("pendulum_2_bd1")

bd1.parentHinge().subhinge(0).setQ(0.2)
bd1.parentHinge().subhinge(0).setU(0.2)

p2_bd1.parentHinge().subhinge(0).setQ(-0.2)
p2_bd1.parentHinge().subhinge(0).setU(-0.2)

We need to use the ConstraintKinematicsSolver now to solve for a new pose. By default, this is allowed to change any coordinate. However, we want to keep the pose for the two subhinges we just set. This is not strictly necessary to finish the assignment; however, it is something useful to know how to do, so we show it here. We’ll freeze the coordinates we don’t want the ConstraintKinematicsSolver to change, and then unfreeze them once we have our pose set.

sim.mb.cks().freezeCoord(bd1.parentHinge().subhinge(0), 0)
sim.mb.cks().freezeCoord(p2_bd1.parentHinge().subhinge(0), 0)
assert sim.mb.cks().solveQ() < 1e-8
assert sim.mb.cks().solveU() < 1e-8
sim.mb.cks().clearFrozenCoords()

Let’s take a look at our system now that the pose is set.

sim.mb.getScene().update()

We also need to set this state on the StatePropagator

sim.sp.ensureHealthy()

# We need to call a hardReset, since the number of states has changed
sim.sp.hardReset()

# Finally, set the state itself
sim.sp.setState(sim.sp.assembleState())

Adding gravity#

Now, we will add gravity using a Gravity KModel.

uniform_gravity = kmdl.UniformGravity("uniform_gravity")
uniform_gravity.setGravity(
    np.array([0.0, 0.0, -9.81]) * ureg.meter / ureg.second**2,
    10.0,
    kmdl.OutputUpdateType.PRE_DERIV,
)
kmdl.Gravity("grav_model", sim.sp, uniform_gravity, sim.mb)

<Karana.Models.Gravity at 0x753eb1dc34b0>

Simulate for 10 seconds#

Finally, we will simulate the system for 10 seconds. While not strictly necessary, we are also adding a SyncRealTime KModel and maximum step size. Otherwise, the simulation runs too fast to see in the GUI.

kmdl.SyncRealTime("sync_real_time", sim.sp, 1.0)
sim.sp.setMaxHopSize(0.1)
sim.sp.advanceBy(10)

<SpStatusEnum.REACHED_END_TIME: 1>

Cleanup#

def cleanup():
    """Clean up the simulation."""
    global sim, sg_ds, new_sg, uniform_gravity, bd1, p2_bd1, p1_bd3, p2_bd3, lc, cn1, cn2
    del sim, sg_ds, new_sg, uniform_gravity, bd1, p2_bd1, p1_bd3, p2_bd3, lc, cn1, cn2


atexit.register(cleanup)

<function __main__.cleanup()>
Contents