Flexible Two-Link Pendulum from Explicit Modal Data

This notebook walks you through creating and running a flexible two-link pendulum using embedded mass properties and modal data. No FEMBridge H5 file or external model resources are needed.

Complete the Flexible Two-Link Pendulum from FEMBridge H5 File before working through this example. That tutorial introduces the flexible links and their connections using the FEMBridge H5 reader. Here, we build the same system by supplying those parameters directly.

import atexit

import numpy as np
import Karana.Dynamics as kd
import Karana.Integrators as ki
import Karana.Math as km
import Karana.Models as kmdl
from Karana.KUtils.Sim import Sim
from Karana.Math.Kquantities import ureg

Create the Simulation

As in the Flexible Two-Link Pendulum from FEMBridge H5 File example, use Karana.KUtils.Sim.Sim to manage the Karana.Dynamics.Multibody, Karana.Dynamics.StatePropagator, and Karana.Frame.FrameContainer.

sim: Sim = Sim()

Define the Flexible-Body Data

The cell below contains all the flexible-body data for both links. Stiffness and damping describe each mode’s response. The pnode matrices describe deformation at a link’s attachment to its parent; the onode matrices describe deformation of the parent link where the second link attaches.

Show code cell source

Hide code cell source

stiffness = (
    np.array(
        [
            2.0549859619140625e02,
            8.2208087158203125e02,
            1.5590296630859375e03,
            4.2521279296875000e03,
            5.9859155273437500e03,
            6.2461474609375000e03,
            1.0984625000000000e04,
            1.6334771484375000e04,
            1.7013275390625000e04,
            2.3981865234375000e04,
            3.6454070312500000e04,
            3.8421781250000000e04,
            4.4583210937500000e04,
            6.5733140625000000e04,
            6.8271796875000000e04,
            7.0989156250000000e04,
            1.0067981250000000e05,
            1.0831357031250000e05,
            1.2580945312500000e05,
            1.4629000000000000e05,
            1.5550515625000000e05,
            2.0705278125000000e05,
            2.4092304687500000e05,
            2.8475368750000000e05,
            3.6889328125000000e05,
            3.7765946875000000e05,
            1.0977122500000000e06,
            1.3563998750000000e06,
            2.0293360000000000e06,
            2.0319782500000000e06,
        ]
    )
    * 1
    / ureg.second**2
)

damping = (
    np.array(
        [
            1.4335222244262695,
            2.8671956062316895,
            3.9484550952911377,
            6.520833492279053,
            7.736869812011719,
            7.903256893157959,
            10.480755805969238,
            12.780755996704102,
            13.043495178222656,
            15.486079216003418,
            19.09294891357422,
            19.60147476196289,
            21.114736557006836,
            25.638473510742188,
            26.128873825073242,
            26.6437931060791,
            31.730085372924805,
            32.91102600097656,
            35.46963119506836,
            38.24787521362305,
            39.434139251708984,
            45.5030517578125,
            49.08391189575195,
            53.3623161315918,
            60.7365837097168,
            61.45400619506836,
            104.7717514038086,
            116.4645767211914,
            142.4547576904297,
            142.5474853515625,
        ]
    )
    * 1
    / ureg.second
)

pnode_rotational_nodal_matrix = (
    np.array(
        [
            [
                -8.7804180701374357e-14,
                1.3318334065801074e-13,
                1.8897804328343870e-12,
                -1.4149886369705200e00,
                7.8455280466682709e-12,
                1.0688759773114231e-11,
                1.7728558204810341e-11,
                -1.7290732734487335e-10,
                1.4163824319839478e00,
                2.5227823302609309e-11,
                4.3101175406512482e-11,
                -1.4225283861160278e00,
                -7.8806829117983312e-11,
                1.3332267734256220e-08,
                -1.4247089624404907e00,
                -1.5215069482099608e-10,
                4.5725540021024358e-10,
                1.4460003376007080e00,
                -1.3106669083384759e-09,
                -2.5435269446205666e-09,
                -1.4450565576553345e00,
                1.7595263912539849e-08,
                1.6446072947928769e-08,
                3.2748053073883057e00,
                3.4513854980468750e00,
                -1.7104116523114499e-07,
                -6.9197341190374573e-09,
                1.9573762610747281e-09,
                -3.1380192888619263e-10,
                -2.3596394171931934e-08,
            ],
            [
                3.9978453516960144e-01,
                4.8768313606201152e-13,
                6.7652267217636108e-01,
                4.1729653586786153e-12,
                9.4854187965393066e-01,
                -7.6654533415210224e-12,
                -2.5538911263556230e-12,
                -1.2228012084960938e00,
                -1.8501807030890660e-10,
                -6.0502464149791990e-11,
                1.5080687999725342e00,
                2.1977331066125316e-10,
                -6.5942591664924777e-11,
                -7.3056311000740948e-11,
                2.7314042694293050e-10,
                1.7911773920059204e00,
                2.2985376102457877e-10,
                -2.4041439949940013e-09,
                -2.1219370365142822e00,
                -1.6310841566280487e-09,
                9.5187688842202078e-09,
                -2.4142284393310547e00,
                7.9199127256401880e-09,
                3.3329307314033940e-08,
                1.6916922973564397e-08,
                7.5579531522151910e-10,
                1.0055750676585262e-09,
                1.1572696685791016e01,
                6.3097668316913769e-06,
                1.3330304145812988e01,
            ],
            [
                -2.1422994671629891e-14,
                3.9983057975769043e-01,
                -9.9141875264940893e-13,
                -4.1158182005791855e-13,
                2.3157218330405183e-12,
                6.7797201871871948e-01,
                -2.2694472169570989e-12,
                3.1399062427254869e-15,
                -8.7556843719749367e-11,
                9.5145952701568604e-01,
                -1.1117929493709156e-11,
                -6.5505682475541782e-12,
                1.0318101408002267e-11,
                1.2528030872344971e00,
                1.3316149960473922e-08,
                -1.5040831080614936e-10,
                -1.2691611650517132e-10,
                6.5451112411796242e-12,
                1.2883413780251374e-10,
                1.5405853986740112e00,
                4.7240318323815700e-09,
                -4.8095052385122017e-09,
                -2.1795218407838490e-10,
                -4.1114192605995470e-10,
                1.5331005798202568e-08,
                -3.8136291347079521e-10,
                6.5432209968566895e00,
                -6.7400804937278735e-09,
                -8.1601934432983398e00,
                3.8655598473269492e-06,
            ],
        ]
    )
    * 1
    / ureg.kilogram**0.5
    / ureg.meter
)

pnode_translational_nodal_matrix = (
    np.array(
        [
            [
                2.7754031898082543e-15,
                -5.0917856729722471e-15,
                2.2923313637915332e-14,
                2.8765171126120259e-14,
                -7.9399635195646912e-14,
                -1.9797106120232610e-13,
                -9.1426663100719452e-02,
                4.2241961339430090e-13,
                2.9904015461795774e-12,
                -3.2055433340279910e-14,
                -2.2486450635544886e-12,
                2.0224353875636369e-12,
                -9.2606998980045319e-02,
                -3.1051075339433298e-12,
                2.7231760249546433e-11,
                5.3401428244670424e-12,
                -9.3051664531230927e-02,
                3.5926265434804705e-11,
                -5.2582043537016676e-13,
                -1.2796286382940614e-12,
                -2.7264185353814696e-10,
                -3.7048333845213222e-10,
                -1.6954097151756287e-01,
                -6.8241634565424647e-11,
                1.0004707284849701e-08,
                1.8491511046886444e-01,
                -1.0605829409149337e-10,
                -2.0018814383959693e-10,
                -2.5507124190582431e-10,
                1.4170779133459988e-11,
            ],
            [
                1.7857731352682871e-14,
                -1.2895497679710388e-01,
                5.0469296710570211e-13,
                1.4830003273579723e-13,
                2.2143495804344271e-13,
                -1.2917165458202362e-01,
                9.5749124774668903e-13,
                -3.2501479806101852e-12,
                2.3330729709281073e-11,
                -1.2931926548480988e-01,
                6.8807234215872981e-12,
                1.1152834298450154e-12,
                -2.4501425194278781e-12,
                -1.3287858664989471e-01,
                -1.4815725313255257e-09,
                2.6213401241315104e-11,
                1.5548331719350550e-11,
                -5.6011757731955214e-12,
                -3.3139300331663790e-11,
                -1.3302773237228394e-01,
                -9.6526875381641730e-10,
                5.6235799350545790e-10,
                6.4579513611717765e-12,
                5.0465747032657493e-12,
                -1.9988291022343674e-09,
                5.2925268334558240e-11,
                -2.4544046819210052e-01,
                1.7036156929606250e-09,
                2.5699537992477417e-01,
                -1.2084851164217980e-07,
            ],
            [
                1.2893499433994293e-01,
                5.6514875109358806e-14,
                1.2874217331409454e-01,
                1.3768617322662546e-12,
                1.2868846952915192e-01,
                -1.8167339377664349e-12,
                -1.9923957774413414e-13,
                -1.2851846218109131e-01,
                -2.4721573746444570e-11,
                -6.4888836491527879e-12,
                1.2940323352813721e-01,
                2.9212219448959686e-11,
                -5.9720158505915943e-12,
                -5.1810587070599823e-12,
                5.4645839936418028e-11,
                1.2924693524837494e-01,
                1.3108258402338979e-11,
                -1.8804607870848145e-10,
                -1.3234081864356995e-01,
                -1.0863067390065595e-10,
                7.5034894964076670e-10,
                -1.3154146075248718e-01,
                4.3030842777902478e-10,
                1.5404281183961643e-09,
                1.1600880256423807e-09,
                5.4484788208908341e-11,
                -7.9801554253577933e-10,
                2.7184343338012695e-01,
                1.2937020699155255e-07,
                2.7561143040657043e-01,
            ],
        ]
    )
    * 1
    / ureg.kilogram**0.5
)

onode_rotational_nodal_matrix = (
    np.array(
        [
            [
                -8.6583287739992743e-14,
                1.7627124261100635e-13,
                -1.0863175593442409e-12,
                1.4149886369705200e00,
                7.9129281252421357e-12,
                1.0983847512080480e-11,
                1.4612895962717865e-11,
                -1.7806878194193132e-10,
                1.4163824319839478e00,
                3.7916628170542310e-11,
                2.7872498146314095e-12,
                1.4225283861160278e00,
                -7.1291765893488623e-12,
                1.3369689355613446e-08,
                -1.4247089624404907e00,
                -1.6345905184955711e-10,
                -4.6855302970882917e-10,
                -1.4460003376007080e00,
                -1.2006163840894146e-09,
                -3.0329854183719362e-09,
                -1.4450565576553345e00,
                -1.6474725583748295e-08,
                1.7620140013718810e-08,
                -3.2748048305511475e00,
                3.4513862133026123e00,
                -1.7238281202480721e-07,
                -7.7800326181431956e-09,
                8.5113271985903793e-10,
                -3.3836883295279563e-10,
                2.2904204755036517e-08,
            ],
            [
                -3.9978453516960144e-01,
                2.4559615951179337e-13,
                6.7652267217636108e-01,
                4.5209933886847242e-12,
                -9.4854187965393066e-01,
                2.7858501096267307e-12,
                2.2444233971352645e-12,
                -1.2228012084960938e00,
                -1.4732919051407833e-10,
                -6.4562050150485106e-11,
                -1.5080687999725342e00,
                1.6435636185363478e-10,
                9.7319487538105598e-12,
                -4.8732212709623468e-11,
                -4.7446135909012810e-10,
                1.7911773920059204e00,
                8.4595053184699509e-11,
                -2.0604216111053120e-09,
                2.1219370365142822e00,
                -2.1892343493590261e-09,
                -7.9673005970448685e-09,
                -2.4142284393310547e00,
                7.9014794707177316e-09,
                3.2964663887469214e-08,
                -8.2604998397073359e-09,
                1.1526696264141378e-09,
                -1.1792337772931205e-08,
                -1.1572696685791016e01,
                6.3096549638430588e-06,
                1.3330304145812988e01,
            ],
            [
                -6.5415494778008152e-15,
                -3.9983057975769043e-01,
                8.1797470272843009e-13,
                -1.8824607942325572e-13,
                2.7387419589131046e-12,
                6.7797201871871948e-01,
                -1.2075339643133343e-12,
                2.2185623130277499e-11,
                -6.7277079740524215e-11,
                -9.5145952701568604e-01,
                -7.5160121182360484e-12,
                1.9983395146971894e-11,
                -4.6109736038038585e-13,
                1.2528030872344971e00,
                1.3345824001476103e-08,
                -1.6329573804263475e-10,
                -8.4735524152890207e-11,
                4.9959231196439191e-11,
                -1.2323862763619076e-10,
                -1.5405853986740112e00,
                1.3525973230343880e-08,
                1.7289771836459522e-09,
                3.0440698572942182e-11,
                2.4196064596360145e-10,
                1.8548199420820310e-08,
                -1.4224420252784142e-10,
                6.5432209968566895e00,
                -6.8832544108943239e-09,
                8.1601934432983398e00,
                -3.8644657252007164e-06,
            ],
        ]
    )
    * 1
    / ureg.kilogram**0.5
    / ureg.meter
)

onode_translational_nodal_matrix = (
    np.array(
        [
            [
                2.9427273487582896e-15,
                -4.0225819128837745e-15,
                1.1849583509240032e-14,
                6.2739181525776205e-14,
                1.0581886895324934e-14,
                -5.1359531048639911e-14,
                9.1426663100719452e-02,
                -2.3032300350799007e-13,
                -1.6042896178181110e-12,
                1.2740215783388353e-13,
                -1.7914277916988852e-12,
                2.3628841938627687e-12,
                -9.2606998980045319e-02,
                9.8308275582603688e-13,
                1.0472090208879514e-11,
                -1.1178382438442602e-12,
                9.3051664531230927e-02,
                -3.3235192375968836e-11,
                1.8714580232725986e-11,
                -2.5711592143567996e-12,
                -2.0022128399688199e-10,
                -3.9000899731966854e-10,
                -1.6954097151756287e-01,
                -9.0685401896162254e-12,
                -7.8951849502573168e-09,
                -1.8491511046886444e-01,
                -1.6235338073933292e-10,
                -2.2078622263776992e-10,
                -2.9524604983066638e-10,
                3.4076808042016182e-10,
            ],
            [
                -3.0964722990144177e-15,
                -1.2895497679710388e-01,
                1.1582729675559122e-13,
                -1.2900367352044334e-13,
                5.0861116533684747e-14,
                1.2917165458202362e-01,
                -1.4665182859318476e-13,
                5.4946173479175631e-12,
                -2.4108966559244216e-11,
                -1.2931926548480988e-01,
                -4.6050494147131804e-12,
                5.0170835368124056e-12,
                2.1023911778161875e-12,
                1.3287858664989471e-01,
                1.4958071448134547e-09,
                -2.8242883726159462e-11,
                -6.8265189508143198e-12,
                1.1269198465016506e-12,
                2.7932298401339706e-12,
                -1.3302773237228394e-01,
                1.8008662339141779e-09,
                1.9832317732504379e-10,
                4.3102885843859795e-11,
                1.1691965971782903e-11,
                2.4208715032614236e-09,
                -1.5542650499966726e-11,
                2.4544046819210052e-01,
                -6.4333072202771291e-10,
                2.5699537992477417e-01,
                -1.2385028469452664e-07,
            ],
            [
                1.2893499433994293e-01,
                2.6642108454815773e-14,
                -1.2874217331409454e-01,
                -1.4349850517916818e-12,
                1.2868846952915192e-01,
                1.3476874238978198e-13,
                -4.6366773268080586e-13,
                1.2851846218109131e-01,
                1.1438605271307800e-11,
                9.6095605431334441e-12,
                1.2940323352813721e-01,
                -2.0630334246485305e-11,
                -2.8843188688149057e-12,
                8.6330265852696542e-12,
                6.3978364273076238e-11,
                -1.2924693524837494e-01,
                -1.3816107112540887e-11,
                1.4823016425413726e-10,
                -1.3234081864356995e-01,
                1.5347030590806554e-10,
                6.3881455680814270e-10,
                1.3154146075248718e-01,
                -3.9892997238943906e-10,
                -1.5325606339544606e-09,
                6.3776822711858472e-10,
                -4.6185048840907683e-11,
                1.7030360455194682e-10,
                2.7184343338012695e-01,
                -1.3205570326135785e-07,
                -2.7561143040657043e-01,
            ],
        ]
    )
    * 1
    / ureg.kilogram**0.5
)

Create the First Link

Karana.Dynamics.PhysicalModalBodyParams collects the mass properties, hinge parameters, and modal data for a flexible body.

Use Karana.Dynamics.PhysicalHingeParams to define a revolute hinge about the y-axis. Karana.Dynamics.ParentPhysicalBodyAttachmentParams places the first link’s attachment at the virtual root. The pnode is at one end of the beam.

Create a Karana.Dynamics.PhysicalModalBody, connect it with a Karana.Dynamics.PhysicalHinge, and apply the parameters using setParams().

link_1_params: kd.PhysicalModalBodyParams = kd.PhysicalModalBodyParams(
    # 'link_1' mass properties
    spatial_inertia=km.SpatialInertia(
        240.0 * ureg.kilogram,
        np.array([0.75, 0.0, -0.0]) * ureg.meter,
        np.array(
            [
                [1.0000079870224, 0.0, 0.0],
                [0.0, 180.03599548339844, 0.0],
                [0.0, 0.0, 180.03599548339844],
            ]
        )
        * ureg.kilogram
        * ureg.meter**2,
    ),
    # Define the attachment of 'link_1' to its parent body
    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.0]) * ureg.meter,
    ),
    # define the 'link_1' 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,
            ),
        ],
    ),
    # Define the location and orientation of the 'link_1' parent hinge
    # relative to the parent body's frame
    parent_body_attachment_params=kd.ParentPhysicalBodyAttachmentParams(
        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,
        )
    ),
    scene_part_specs=[],
    scene_file_object_specs=[],
    nmodes=30,
    stiffness=stiffness,
    damping=damping,
    # Rotational nodal matrix for the body's pnode. Size (3 x 30)
    pnode_rotational_nodal_matrix=pnode_rotational_nodal_matrix,
    # Translational nodal matrix for the body's pnode. Size (3 x 30)
    pnode_translational_nodal_matrix=pnode_translational_nodal_matrix,
)

# Create the 'link_1' body
link_1: kd.PhysicalModalBody = kd.PhysicalModalBody.create("link_1", sim.mb, 30)

# Connect the 'link_1' body to its parent via a hinge
kd.PhysicalHinge(sim.mb.virtualRoot(), link_1, kd.HingeType.REVOLUTE)

# Set the parameters for the 'link_1' body
link_1.setParams(link_1_params)

Attach the Second Link

Deep-copy the first link’s parameters to reuse the identical beam data. Replace the parent attachment with Karana.Dynamics.ParentPhysicalModalBodyAttachmentParams, which includes the far-end transform and the parent’s onode matrices. The second link keeps its own near-end pnode matrices.

This is the equivalent to adding the parent attachment with the FEMBridge reader in the Flexible Two-Link Pendulum from FEMBridge H5 File example.

link_2_params = link_1_params.model_copy(deep=True)
# Define the location and orientation of the 'link_2' parent hinge
# relative to the parent body's frame
link_2_params.parent_body_attachment_params = kd.ParentPhysicalModalBodyAttachmentParams(
    inb_to_joint_transform=km.HomTran(
        km.UnitQuaternion(np.array([0.0, 0.0, 0.0, 1.0])),
        np.array([1.5, 0.0, 0.0]) * ureg.meter,
    ),
    onode_rotational_nodal_matrix=onode_rotational_nodal_matrix,
    onode_translational_nodal_matrix=onode_translational_nodal_matrix,
)

# Create the 'link_2' body
link_2: kd.PhysicalModalBody = kd.PhysicalModalBody.create("link_2", sim.mb, 30)

# Connect the 'link_2' body to its parent via a hinge
kd.PhysicalHinge(link_1, link_2, kd.HingeType.REVOLUTE)

# Set the parameters for the 'link_2' body
link_2.setParams(link_2_params)
del link_1_params, link_2_params

# Finalize the multibody for the added bodies
sim.mb.ensureHealthy()

Add Gravity

Use Karana.Models.UniformGravity and Karana.Models.Gravity to apply gravity in the negative z-direction.

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

Set the Initial State

Reset the multibody and set the hinge angles to 110° and 15°. Both links start at rest with zero modal coordinates, so releasing them produces swinging motion and flexible deformation.

sim.mb.resetData()
link_1.parentHinge().subhinge(0).setQ(110.0 * ureg.deg)
link_2.parentHinge().subhinge(0).setQ(15.0 * ureg.deg)

True

Configure the Integrator

Here, we configure the integrator. The performance of flexible-body problems in particular is sensitive to the integrator chosen. kdFlex offers a large variety of integrators and options to meet simulations’ needs. Here, SUNDIALS’s ARKODE’s implicit ESDIRK45 method does well.

integrator_options = ki.ArkImplicitIntegratorOptions()
integrator_options.rtol = 1e-5
integrator_options.atol = 3e-8
integrator_options.max_nl_iters = 5
sim.sp.setIntegrator(ki.IntegratorType.ARKIMPLICIT_ESDIRK45, integrator_options)

sim.sp.ensureHealthy()
sim.sp.hardReset()
sim.sp.setState(sim.sp.assembleState())

# Here, a maximum hop size is added so we get visualization updates every 1/60 seconds
sim.sp.setMaxHopSize(1.0 / 60.0)

Open the kdFlex Viewer

Use setupGui() to open the viewer and set the camera position. This cell is optional if you only want to run the dynamics.

This standalone model includes the joint coupling matrices, but no sensor nodes along the beams. Its stick view shows the connected body and joint frames, but graphics do not show the detailed bending shape like in the Flexible Two-Link Pendulum from FEMBridge H5 File example. This motions affect on the dynamics is still captured numerically, but it is just not shown graphically.

# Set the stick parts to be a bit larger so they are easier to see
sim.mb.createStickParts()
sim.mb.scaleStickParts(5.0)

# Create the GUI and set the camera location
sim.setupGui(port=0)
target = sim.mb.virtualRoot().frameToFrame(link_2).relTransform().getTranslation().m
sim.mb.getScene().viewAroundFrame(
    sim.mb.virtualRoot(),
    target + np.array([0.0, 6.0, 0.0]),
    target,
    np.array([0.0, 0.0, 1.0]),
)

[WebUI] Karana Viewer is running on port 35899
Open this URL to connect:
	http://newton:35899

Click to open Karana Viewer in a new tab

Run the Simulation

Advance to 10 seconds.

sim.sp.advanceTo(10.0)

<SpStatusEnum.REACHED_END_TIME: 1>

Clean Up the Simulation

Below, we cleanup our simulation.

def cleanup() -> None:
    """Cleanup the simulation."""
    global link_1, link_2, sim
    del link_1, link_2, sim


atexit.register(cleanup)

<function __main__.cleanup() -> None>