Flexible Double-Wishbone

Flexible Double-Wishbone#

This example builds on the rigid double-wishbone. This example and makes one of its links flexible, adds a spring-damper, and runs a full simulation of the double-wishbone system. Similar to the Flexible slider crank, this is an example of a multibody that has flexible bodies and constraints.

In this example, the multibody has already been prepared and saved to an H5 file, so it is easy to create. The Flexible slider crank shows how a simulation with flexible bodies can be built of procedurally.

Requirements:

In this tutorial we will:

  • Import the Multibody

  • Initialize the state and constraints

  • Setup the kdFlex Scene

  • Setup the state propagator and models

  • Run the simulation

  • Clean up the simulation

import atexit
import numpy as np
from numpy.typing import NDArray
from typing import cast
from Karana.Core import discard, allReady
from Karana.Frame import FrameContainer
from Karana.Dynamics import PhysicalModalBody
from Karana.Math import IntegratorType
from Karana.Dynamics import (
    StatePropagator,
    PhysicalBody,
    PhysicalSubhinge,
    MMSolverType,
    Algorithms,
)
from Karana.Dynamics.SOADyn_types import MultibodyDS
from Karana.Math import NonlinearSolver, SolverType, SpatialVector
from Karana.Models import (
    UpdateProxyScene,
    ProjectConstraintError,
    SubhingeSpringDamper,
)

Import the multibody#

Here, we create the multibody. It is already saved as an H5 file, so we can just load it in via a SOADyn_types.MultibodyDS DataStruct.

fc = FrameContainer("root")
mbody_ds = MultibodyDS.fromFile("../resources/double_wishbone/flexible_double_wishbone.h5")
mb = mbody_ds.toMultibody(fc)
del mbody_ds

Initialize the State and Constraints#

We are going to initialize the state of this system. This system contains both constraints and flexible bodies. For the constraints, we will need to use a Karana.Dynamics.ConstraintKinematicsSolver, just as we did in the Rigid double-wishbone example.

We could finish the state initialization here. However, when we first start moving, there may be some transient behavior, as the flexible body itself is not in the correct deformed state. Here, we create a simple equilibrium solver to deform the flexible body such that it’s initial modal coordinate acceleration is zero. In this case, the solution is trivial, as we are starting from rest, but this showcases how it can be done in general.

Satisfying the constraints#

We being by setting an initial guess for the Karana.Dynamics.ConstraintKinematicsSolver. This is done by setting all joints, body coordinates, and constraint coordinates to 0.

mb.ensureHealthy()
mb.resetData()

# Set the constraint data to zero
ccd = mb.cutjointCoordData()
ccd.setQ(0)
ccd.setU(0)
ccd.setUdot(0)
ccd.setT(0)
del ccd

# Set the body coordinate data to zero
bcd = mb.bodyCoordData()
bcd.setQ(0)
bcd.setU(0)
bcd.setUdot(0)
bcd.setT(0)
del bcd

Now, we use the Multibody’s ConstraintKinematicsSolver to solve for coordinates and velocities that satisfy the constraints. We want lower_control_arm’s subhinge to remain unchanged, so we freeze this coordinate. This effectively removes it from the coordinates that the ConstraintKinematicsSolver is allowed to change.

bd = cast(PhysicalBody, mb.getBody("lower_control_arm"))
mb.cks().freezeCoord(bd.parentHinge().subhinge(0), 0)

# Solve for joint angles that satisfy the constraints
assert mb.cks().solveQ() < 1e-10
assert mb.cks().solveU() < 1e-10

Ensure everything is ready before continuing.

# Set the gravity and external forces to zero
mb.setUniformGravAccel(np.zeros(3))
cast(PhysicalBody, mb.getBody("wheel")).getNode("WheelContactNode").setExternalSpForce(
    SpatialVector(np.zeros(3), np.zeros(3))
)

# Ensure everything has been set and is ready.
allReady()

[warning] Parameter setpoint is not ready.
[warning] Object 'shock_absorber_spring_params ({py:class}`Karana.Core.Base`)' with id=2566 is not ready.

False

Equilibrium#

As mentioned earlier, we solve for the flexible body’s equilibrium. Here, the solution is trivial since we are starting from rest (so the solution should be a vector of all zeros). Regardless, we include it here to showcase how solving for equilibrium can be done.

To solve for equilibrium we use a Karana.Math.NonlinearSolver. These solvers will be covered in detail in an upcoming example. For now, it is sufficient to note that they take in a function that takes in a guess for the current solution, x, and outputs values, v for that guess. The values must be modified in-place, which is why the code below uses v[:] = ... rather than v = .... Here, our equilibrium function is simple: it takes in a guess for the flexible body coordinates, q, of upper_control_arm, runs the forward dynamics, and outputs the modal accelerations of upper_control_arm. The nonlinear solver tries to drive these outputs to zero.

uca_bd = cast(PhysicalModalBody, mb.getBody("upper_control_arm"))


def equilibrium(x: NDArray, v: NDArray) -> None:
    """Solve for equilibrium.

    The incoming x values will be the new Q states of the beam.
    The output v values should be the accelerations of the beam as a
    result of the incoming Q values.

    Parameters
    ----------
    x : NDArray
        New guess for beam Q values.
    v : NDArray
        Udot values of the beam after running the dynamics.
    """
    uca_bd.setQ(x)
    Algorithms.evalForwardDynamics(mb)
    v[:] = uca_bd.getUdot()


solver = NonlinearSolver("solver", uca_bd.nQ(), uca_bd.nU(), equilibrium)
solver.createSolver(SolverType.HYBRID_NON_LINEAR_SOLVER)
q = np.array(uca_bd.getQ())
solver.solve(q)

uca_bd.setQ(q)

Similar to the constraint solution, we can again check that the solution here is sufficient.

udot = np.empty(q.size)
equilibrium(q, udot)
assert np.linalg.norm(udot) < 1e-10

del uca_bd
del solver

Setup the kdFlex Scene#

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.

cleanup_graphics, web_scene = mb.setupGraphics(port=0, axes=0.5)
web_scene.defaultCamera().pointCameraAt(
    np.array([1.5, -0.2, 0.0]), np.array([0.0, -0.2, 0.0]), np.array([0.0, 0.0, -1.0])
)
mb.createStickParts()
del web_scene

[WebUI] Listening at http://newton:35609

Setup the State Propagator and Models#

As in other examples, we use a Karana.Dynamics.StatePropagator to drive the simulation.

# Set up state propagator using the cvode_stiff integrator.
sp = StatePropagator(
    mb, IntegratorType.CVODE_STIFF, None, None, MMSolverType.TREE_AUGMENTED_DYNAMICS
)

# Initialize state.
t_init = np.timedelta64(0, "ns")
sp.setTime(t_init)
sp.setState(sp.assembleState())

# Makes sure the visualization scene is updated after each state change.
UpdateProxyScene("update_proxy_scene", sp, mb.getScene())

Shock absorber#

Register a model for the shock absorber.

ssd = SubhingeSpringDamper(
    "shock_absorber_spring",
    sp,
    cast(PhysicalSubhinge, mb.getBody("shock_absorber_upper").parentHinge().subhinge(0)),
)
ssd.params.k = 300
ssd.params.d = 100
ssd.params.setpoint = np.array([0.0])
del ssd

Constraint error projection#

Even though the individual forward dynamics calls themselves should not introduce much constraint error, as we integrate, constraint error will build up. Hence, periodically, we need to use a method to eliminate this error. Here, we use the Karana.Models.ProjectConstraintError model, which uses a ConstraintKinematicsSolver to project out the error once it gets too large.

pce = ProjectConstraintError("project_error", sp, mb, sp.getIntegrator(), mb.cks())
pce.params.tol = 1e-8
h = np.timedelta64(int(1e7), "ns")
pce.setPeriod(h)
del pce

Prescribing upper_control_arm acceleration#

Currently, our simulation won’t move, as nothing is changing the acceleration of the prescribed hinge for upper_control_arm. Here, we add a function that will modify the acceleration so that we increase the velocity for the first two seconds.

# Move the system
t_r = 4.0


def move(t_in, _):
    """Apply prescribed Udot to move the system.

    Parameters
    ----------
    t_in : float
        The current time.
    _ : NDArray[np.float64]
       The current state (unused).
    """
    t = float(t_in) / 1e9
    if t > t_r:
        accel = -10 * np.pi / 180 * np.sin(t)
    else:
        accel = (
            10
            * np.pi
            / 180
            * (
                (6 / t_r**2 - 12 * t / t_r**3) * np.sin(t)
                + 2 * (6 * t / t_r**2 - 6 * t**2 / t_r**3) * np.cos(t)
                - (3 * (t / t_r) ** 2 - 2 * (t / t_r) ** 3) * np.sin(t)
            )
        )
    bd.parentHinge().subhinge(0).setUdot(accel)


sp.fns.pre_deriv_fns["move"] = move

Run the simulation#

sp.advanceTo(4 * np.pi)

<SpStatusEnum.REACHED_END_TIME: 1>

Clean Up the Simulation#

def cleanup():
    """Cleanup the simulation."""
    import gc

    gc.collect()
    global bd
    del bd

    discard(sp)
    cleanup_graphics()

    discard(mb)
    discard(fc)


atexit.register(cleanup)

<function __main__.cleanup()>

Summary#

You now understand how to set the state and constraints, solve them, and register the shock, constraint error, and acceleration models to make the simulation work.

Further Readings#

Contents