Recipes

Contents

Recipes#

Frames layer#

Multibody systems typically have a large family of frames defined by bodies, nodes on bodies, hinge attachment points, and other locations of interest such as end-effectors, actuator/sensor mount locations etc. Non-multibody frames can include environmental locations, as well as celestial bodies in motion. The frames layer provides a convenient foundational layer for organizing these frames in a tree structure, and for making arbitrary relative pose, spatial velocity and acceleration queries across them. The Frame class forms the base class for bodies, nodes and celestial body frames.

Creating a new frame#

Frames are created within a frame container instance. You can use fc  = FrameContainer("root") to create a new frame container with just a single root frame. A new frame can be created in a frame container fc using new_f = Frame.create("new_name", fc). At this point the new frame is unattached to the frame tree, and not especially usable. the frame can be added to the frame tree by attaching it to a frame already in the tree. The new frame new_f can be attached to such a frame f by creating a new frame-to-frame edge via new_edge =  PrescribedFrameToFrame.create(f, new_frame).

Finding a frame instance#

  • Given a frame container fc, a frame instance can be looked up using its name via f = fc.getFrame(frame_name).

  • the root frame for the frames tree can be looked up using f = fc.root().

  • Each physical body is derived from a frame, and this frame represents the body frame.

  • Each node is derived from a Frame, and this frame represents the local node frame.

  • Each f2f frame-to-frame instance describes the relative motion of a pair of frames, and the from frame can be obtained by a call to f2f.oframe(), and the to frame by a call to f2f.pframe().

  • The physical subhinges class is derived form the frame-to-frame class, so the above methods can be used to get access to its from and to frames. The same applies to frame-pair hinge and physical hinge classes.

  • The celestial frames are SpiceFrame instances, and thus also frame instances themselves.

  • Environmental geometry objects are typically instances of proxy scene node and proxy scene part instances. The objects have a Karana.Scene.ProxySceneNode.ancestorFrame() method to access the frame instance representing their local frames.

The building blocks of a multibody system are intimately tied to the frames layer, and there is rick interplay between them.

Frame to frame data#

Given a pair of frames A and B, the frame-to-frame instance relating the two can be obtained via f2f = A.frameToFrame(B).

  • The relative pose of frame B with respective to frame A can be obtained vi A_T_B = f2f.relTransform().

  • The relative spatial velocity of frame B with respective to frame A as observed from A and represented in the A frame can be obtained via V_AB = f2f.relSpVel().

  • The relative spatial velocity of frame B with respective to frame A as observed from B and represented in the B frame can be obtained via V_AB = f2f.pframeObservedRelSpVel().

  • The relative spatial acceleration of frame B with respective to frame A as observed from A and represented in the A frame can be obtained via A_AB = f2f.relSpAccel().

  • The relative spatial acceleration of frame B with respective to frame A as observed from B and represented in the B frame can be obtained via V_AB = f2f.pframeObservedRelSpAccel().

Body hinges and subhinges are frame-pair hinge and physical subhinges classes which are derived from the frame-to-frame classes, and thus the above methods can be used with them directly to obtain relative information across these frame-to-frame instances.

Changing observer (derivative) frame#

  • Given a candidate oframe observed and represented relative spatial velocity V_oframe, the equivalent pframe observed and represented relative spatial velocity can be obtained via V_pframe = f2f.toPframeDerivSpVel(V_oframe). Note that V_oframe does not have to be actually the current relative spatial velocity across the f2f.

  • Given a candidate oframe observed and represented relative spatial acceleration A_oframe, the equivalent pframe observed and represented relative spatial acceleration can be obtained via A_pframe = f2f.toPframeDerivSpAccel(A_oframe). Note that A_oframe does not have to be actually the current relative spatial acceleration across the f2f.

  • More generally, a 3-vector x_A that is a derivative observed and represented in frame A, can be converted into a B frame observed and represented frame via x_B = B.frameToFrame(A).toOframeObserved(x_A).

Reattaching a frame#

A frame, and its descendant frame tree, can be reattached to a different frame by creating a new edge frame-to-frame between the new parent frame and the frame as described in the Creating a new frame section. The old parent edge is automatically deleted.

Deleting a frame#

Like most objects, a frame f can be deleted by calling the Karana.KCore.discard() method as discard(f). Note that this method will raise an error if f has children frames. The children frames must detached, reattached elsewhere or deleted before f can be deleted.

Rigid multibody models#

Create rigid body serial-chain multibody system (manual)#

To create a serial chain multibody system one body at a time, you would follow a manual approach, which provides granular control over each body and its connections, as opposed to procedural methods like Karana.Dynamics.PhysicalBody.addSerialChain() that create multiple uniform bodies in bulk.

To create a serial chain multibody system one body at a time, you would follow a manual approach, which provides granular control over each body and its connections, as opposed to procedural methods like Karana.Dynamics.PhysicalBody.addSerialChain() that create multiple uniform bodies in bulk.

Here’s a recipe outline for manually creating a serial chain multibody system:

  1. Initialize the Multibody System:

    • Begin by creating a frame container (e.g., fc = FrameContainer("root")). This serves as the root for your system.

    • Instantiate a multibody object, associating it with the created frame container (e.g., mb = Multibody("mb", fc)).

  2. Define General Body and Joint Parameters (as needed):

    • Spatial Inertia (spI): Define the mass and inertia properties for your bodies. For example, spI = SpatialInertia(2.0, np.zeros(3), np.diag([3, 2, 1])).

    • Joint Transforms (body2joint_T, inb2joint_T): Specify the transformations from the body’s frame to its joint and from the inboard body’s frame to the joint. For instance, body2joint_T=HomTran(np.array([0, 0, 0.5])) and inb2joint_T=HomTran(np.array([0, 0, -0.5])).

    • Visual Geometries (scene part spec): Define specifications for how the body and its pivot points should appear in a graphics scene (e.g., sp_body, sp_pivot with geometry, material, and transform). These can be grouped into scene_part_specs within physical body.

    • (Optional) Visual Geometries (scene part spec): If visual representations are needed, define scene part spec objects with geometry, material, and transform) and add them to scene_part_specs.

  3. Create and Configure the First Body:

    • Create the body: Call the Karana.Dynamics.PhysicalBody.create() method, providing a unique name for the body and the multibody instance it belongs to (e.g., bd1 = PhysicalBody("bd1", mb)).

    • Set Spatial Inertia: Assign the defined spatial inertia to the body (e.g., bd1.setSpatialInertia(spI)).

    • Define Joint Connection: Create a physical hinge to connect this first body to the multibody’s virtualRoot(). Specify the hinge type (e.g., HingeType.PIN or HingeType.BALL) and the parent and child bodies (e.g., bd1_hge = PhysicalHinge(cast(PhysicalBody, mb.virtualRoot()), bd1, HingeType.PIN)).

    • Set Body-to-Joint Transform: Define the transform from the body’s frame to its joint (e.g., bd1.setBodyToJointTransform(b2j)).

  4. Create and Configure Subsequent Bodies in the Serial Chain:

    • For each additional link in your serial chain, repeat the process from step 3:

      • Create a new body Call the Karana.Dynamics.PhysicalBody.create() method and give the body a )preferably unique) name and associate it with the multibody system.

      • Set Spatial Inertia: Assign its inertia properties.

      • Define Joint Connection: Create a physical hinge to connect this new body to the immediately preceding body in the chain. For example, if bd1 was the first body, the second body (bd2) would connect to bd1 as its parent.

      • Set Body-to-Joint Transform: Define its body-to-joint transform.

  5. Finalize the Multibody System:

    • Once all individual bodies have been created and their hinges established, you must call the Karana.Dynamics.Multibody.ensureCurrent() method on your multibody instance. This step finalizes the setup and prepares the system for dynamics computations.

    • (Optional) You may also use Karana.Dynamics.Multibody.resetData() and Karana.Dynamics.Multibody.displayModel() to inspect the created system.

This manual approach provides “complete control over creating the right body types, hinge types and setting up the parent/child body relationships,” as described in the sources.

Procedurally create rigid body serial chain#

To procedurally create a serial chain multibody system with a user-specified number of bodies, you leverage specialized helper methods like Karana.Dynamics.PhysicalBody.addSerialChain() which are designed for generating multiple bodies in bulk, particularly useful for large test systems or performance benchmarking.

Here’s a recipe outline for procedurally creating a serial chain multibody system:

  1. Carry out the steps 1 and 2 in the Create rigid body serial-chain multibody system (manual) recipe to create a frame container, an empty multibody system and define the properties for each body. These properties will be uniformly applied to all the bodies in the system. You can always modify any of body’s properties later once the bodies have been created. Additional body properties that will be needed include:

    • Define Hinge Axes (axes):

      • Example: axes=[np.array([0.0, 1.0, 0.0])]

    • Set Body-to-Joint Transform (body2joint_T): This defines the transformation from the body’s frame to its joint.

      • Example: body2joint_T=HomTran(np.array([0, 0, 0.5]))

    • Set Inboard Body-to-Joint Transform (inb2joint_T): This defines the transformation from the inboard body’s frame to the joint.

      • Example: inb2joint_T=HomTran(np.array([0, 0, -0.5]))

  2. Create the serial chain:

    • Call the static Karana.Dynamics.PhysicalBody.addSerialChain() method , available on either the physical body or physical modal body class, to add the chain of bodies to your multibody system.

    • Parameters:

      • name: A prefix string for the new body names (e.g., "link" or "body").

      • nbodies: The number of bodies to create in the serial chain (e.g., n_links).

      • root: The parent body to which the chain will be attached. Often, this is the virtualRoot() of your multibody system (e.g., cast(PhysicalBody, mb.virtualRoot())).

      • htype: The type of hinge to use for connecting the bodies (e.g., Karana.Dynamics.HingeType.PIN or Karana.Dynamics.HingeType.BALL). PIN is often the default.

      • params: The physical body object containing the mass properties, joint connection properties, and visual geometries defined in step 1.

    • Example Python Call (for rigid bodies):

      PhysicalBody.addSerialChain(
    "link", n_links, cast(PhysicalBody, mb.virtualRoot()),
    htype=HingeType.PIN, params=params
)

  3. Finalize the Multibody System:

This procedural approach, exemplified by the “n-link pendulum (procedural)” example, automates the creation of large multibody systems by defining parameters once and applying them to many bodies in a chain.

Procedurally create a regular rigid body tree#

The steps for procedurally creating a tree multibody system with a user-specified branching structure are very similar to the steps in the Procedurally create rigid body serial chain recipe, except for using the Karana.Dynamics.PhysicalBody.addTree() method instead of the Karana.Dynamics.PhysicalBody.addSerialChain() method.

The Karana.Dynamics.PhysicalBody.addTree() static method directly adds a sub-tree of rigid physical body instances.

  • Parameters:

    • name: A prefix string to use for naming the new bodies (e.g., "branch_link").

    • branch_length: The number of bodies in each branch of the tree.

    • nbranches: The number of child branches originating from each node.

    • depth: The number of branching levels to create.

    • root: The existing physical body instance to which the new sub-tree will be attached. This is often the multibody’s virtualRoot() (e.g., cast(PhysicalBody, mb.virtualRoot())).

    • htype: The type of hinge to use for connections (e.g., Karana.Dynamics.HingeType.PIN).

    • params: The physical body object containing the uniform properties.

  • Example Python Call:

            PhysicalBody.addTree(
                "branch_link", branch_length=3, nbranches=2, depth=2,
                root=cast(PhysicalBody, mb.virtualRoot()),
                htype=HingeType.PIN, params=params
            )

Create general branched rigid body tree#

The creation of arbitrary tree structure multibody systems can be done

  1. manually body by body using the Create rigid body serial-chain multibody system (manual) recipe and attaching bodies appropriately to build up the desired multibody topology.

  2. alternatively, the procedural techniques in the Procedurally create rigid body serial chain recipe and the Procedurally create a regular rigid body tree recipe can be used repeatedly to create and attach fragments to assemble the desired multibody topology.

Importing and exporting multibody model#

kdflex supports importing and exporting multibody models in the following formats :

  • Import/Export Formats:

  • Import Formats:

    • JPL DARTS dynamics simulation tool Python model format files.

I. Exporting a Multibody:

We assume that a multibody instance, mb has already been created.

  1. Convert to Karana.Dynamics.SOADyn_types.MultibodyDS DataStruct:

  2. Export to Various File Formats:

    • Use the toFile() method on the mb_DS_to_export object, specifying the desired filename with the appropriate extension.

    • To JSON: Example mb_DS_to_export.toFile("2_link_multibody.json")

    • To YAML: Example mb_DS_to_export.toFile("2_link_multibody.yaml")

    • **To HDF5:**  Example

    mb_DS_to_export.toFile("2_link_multibody.hdf5")

    • To URDF: Example mb.toUrdf("output.urdf")

II. Importing a Multibody:

  1. Import the Karana.Dynamics.SOADyn_types.MultibodyDS DataStruct from File:

    • Use the fromFile() method of MultibodyDS to load the saved model file.

    • For JPL DARTS Python model files, you would use Karana.KUtils.darts_model_file_converter.convertDARTSModelFile() first, then convert to MultibodyDS).

    • Example (importing from JSON):

      imported_ds = MultibodyDS.fromFile("2_link_multibody.json")
# You could also import from YAML or HDF5:
# imported_ds = MultibodyDS.fromFile("2_link_multibody.yaml")
# imported_ds = MultibodyDS.fromFile("2_link_multibody.hdf5")

  2. Convert to multibody:

After these steps, the mb_new multibody should be ready for further operations. The importing/exporting process is demonstrated in the Model import/export and URDF import notebooks.

Computing mass properties of a multibody system#

A multibody instance is a also a subtree, and as such all of the methods described in the Computing mass properties of a SubTree section for computing the mass properties for a subtree can also be used by passing the multibody as an argument to the methods described in the section.

How to detach a body from it parent body?#

Here’s a recipe outline for detaching a body in kdFlex:

Recipe Outline: Detaching a Body

A physical body can be detached by calling its detach() method. For example:

    ```python
    body_to_detach.detach()
    ```

It’s important to understand what happens internally when detach() is called:

  • Parent Disconnection: The body is removed from its direct parent in the topological tree.

  • Hinge Deletion: The hinge that previously connected the detached body to its former parent is automatically deleted.

  • New Hinge Creation: A new Karana.Dynamics.HingeType.FULL6DOF type hinge is automatically created.

  • Reattachment to Virtual Root: This newly created FULL6DOF hinge is used to re-attach the detached body directly to the multibody system’s virtualRoot() body. This effectively gives the detached body 6 degrees of freedom relative to the virtual root (ground).

  • State Preservation: Crucially, the coordinates of the new FULL6DOF hinge are initialized such that the detached body’s inertial pose (position and orientation), spatial velocity, and spatial acceleration are preserved. This means the body’s physical state remains continuous and consistent immediately after the detachment operation.

After any operation that alters the system’s topology, it’s good practice to ensure the multibody system’s internal data structures are updated for subsequent dynamics calculations. You can use multibody’s dumpTree to print the new hierarchical structure of your multibody system. This allows you to visually confirm that the body is now connected directly to the _MBVROOT_ (the virtual root).

  • Example:

    mb.dumpTree()

By following these steps, you can successfully detach a physical body from its parent and re-anchor it to the virtual root, thus modifying the multibody system’s topology while ensuring the body’s dynamic state is preserved.

How to reattach a physical body to new parent body?#

To reattach a body to a different parent body, you essentially perform a two-step process: first, you detach the body from its current parent (if it’s not already detached), and then you attach it to the new desired parent body. The physical body reattach() method is designed for this purpose, allowing you to connect a body to a new parent with a specified hinge type and relative transforms.

It’s important to understand what happens internally when reattach() is called:

  • Hinge Deletion: The hinge that previously connected the detached body to its former parent is automatically deleted.

  • New Hinge Creation: A new physical hinge of the specified type is created.

  • Reattachment to new parent: This newly physical hinge is used to re-attach the detached body directly to the new physical body parent body.

  • State Preservation: If the new hinge type is FULL6DOF, then the coordinates of the new physical hinge are initialized such that the reattached body’s inertial pose (position and orientation), spatial velocity, and spatial acceleration are preserved. This means the body’s physical state remains continuous and consistent immediately after the detachment operation. For other hinge types, the user needs to manually set the hinge parameters (eg. axes), the onode and pnode transforms, and the hinge coordinates.

By following these steps, you can effectively reattach a body to a different parent within the multibody system.

Using Bilateral Closure Constraints#

Closure constraints impose motion constraints on the bodies. See the Bilateral closure constraints section for more discussion on the types of closure constraints supported in kdFlex.

Adding hinge loop constraints#

Bilateral loop constraints can be added to the multibody system using the hinge loop constraint class. Such hinge loop constraints internally use a frame-pair hinge hinge instance to define the motion permitted across the constraint. The end point of a loop constraint on a body must be a Karana.Dynamics.ConstraintNode instance. When not connected to a body, the other end can be a Karana.Frame.Frame instance. The recipe for creating a hinge loop constraint is:

  1. Look up or create a constraint node on the body being connected by the loop constraint.

  1. Repeat Step 1 if the other end of the loop constraint is meant to be connected to another body. Else lookup of create a frame. Create a frame-to-frame instance defining the two ends of the loop constraint.

  • Example: f2f = cnode1.frameToFrame(cnode2)

  1. Create the hinge loop constraint using Karana.Dynamics.LoopConstraintHinge.create() and specify the hinge type to use.

  • Example: lc = LoopConstraintHinge.create(mb, f2f, "my_lc", HingeType.PIN)

  1. Now that the loop constraint has been created, it much be activated by calling the Karana.Dynamics.SubGraph.enableConstraint() () method for it to have any effect.

  • Example: mb.enableConstraint(lc)

A loop constraint can be deactivated at any time by using the Karana.Dynamics.SubGraph.disableConstraint() method. Enabled constraints can be looked by name using the Karana.Dynamics.SubGraph.getEnabledConstraint() method.

Addingconvel (non-hinge) loop constraints#

The process for creating a hinge loop constraint is the same for hinge loop constraints described in the Adding hinge loop constraints recipe, except that the Step 3 construction process is changed to:

  1. Create the hinge loop constraint using Karana.Dynamics.LoopConstraintConVel.create().

  • Example: lc = LoopConstraintConVel.create(mb, f2f, "my_lc")

These constraints do not use hinges. However, they require a unit axis parameter which can set using the Karana.Dynamics.LoopConstraintConVel.setUnitAxis() method.

Adding coordinate constraints#

Coordinate constraints impose a fixed scale factor constraint between the coordinates of a pair of physical subhinge (of the same dimension). Nodes are not used by convel loop constraint instances. One can be created using Karana.Dynamics.CoordinateConstraint.create() and specifying the pair of physical subhinge being constrained.

  • Example: cc = CoordinateConstraint.create(mb, oshg, pshg, "my_cc")

The scale factor for a convel loop constraint can be set using the Karana.Dynamics.CoordinateConstraint.setScaleRatio() method.

A convel loop constraint can be deactivated at any time by using the Karana.Dynamics.SubGraph.disableConstraint() method. Enabled constraints can be looked by name using the Karana.Dynamics.SubGraph.getEnabledConstraint() method.

Working with sub-trees#

A Karana.Dynamics.SubTree is a class that represents a sub-tree of connected bodies in a multibody system. It is designed to allow algorithms and computations to be limited to a specific portion of a multibody system. See Sub-Trees section for for details on the subtree and subgraph classes. A subtree is always a part of a multibody system.

Creating a SubTree Instance#

A new subtree instance can be created from an existing subtree instance, such as the multibody instance itself. The bodies to be added to the new subtree instance can be tailored by specifying a new root body, branches to include, and bodies to exclude (see here for details on how to set these arguments). A new subtree can be called at the C++ level using

        auto new_subtree = static kc::ks_ptr<SubTree> create(
            const kc::ks_ptr<SubTree> &parent_subtree,
            std::string_view name,
            const kc::ks_ptr<BodyBase> &new_root,
            std::vector<kc::ks_ptr<BodyBase>> use_branches={},
            std::vector<kc::ks_ptr<BodyBase>> stop_at={}
        )

At the Python level the subtree can be created as follows:

        # Assuming 'mb' is your Karana.Dynamics.Multibody instance
        # and 'some_body' is a `BodyBase` within 'mb'
        # 'some_other_body' could be a leaf in this new subtree
        subtree_name = "my_specific_subtree"
        subtree_root_body = mb.getBody("body_A") # Get a BodyBase pointer from your multibody
        # Optional: Specify use_branches and stop_at arguments to tailor
        #   the bodies added. Using the same value will result in a 
        #   spanning tree of the bodies
        # subtree_stop_at = [mb.getBody("body_B"), mb.getBody("body_C")]

        my_subtree = Karana.Dynamics.SubTree.create(
            parent_subtree,
            subtree_name,
            subtree_root_body,
            # use_branches=subtree_stop_at # uncomment to limit to branches with these bodies 
            # stop_at=subtree_stop_at # uncomment to skip descendants of these bodies 
        )

You can inspect the contents of the new subtree by first making it current by calling its ensureCurrent() method, and then calling its displayModel()and its dumpTree() methods.

Inspecting and Querying a SubTree#

Once a subtree is created, you can access its properties and structure.

  1. Get the Virtual Root Body:

    • The virtualRoot() method returns the body base class root body of the subtree.

    • Recipe Step:

      root_of_subtree = my_subtree.virtualRoot()
print(f"Root of '{my_subtree.name()}' is: {root_of_subtree.name()}")

  2. Get a Body’s Parent within the SubTree:

    • The parentBody(const BodyBase &body) method returns the immediate parent of a specified body within the context of this specific subtree.

    • Recipe Step:

      # Assuming 'child_body_in_subtree' is a BodyBase within 'my_subtree'
child_body_in_subtree = mb.getBody("body_Y") # Must be part of the subtree
parent_in_subtree = my_subtree.parentBody(child_body_in_subtree)
if parent_in_subtree:
    print(f"Parent of '{child_body_in_subtree.name()}' in '{my_subtree.name()}' is: {parent_in_subtree.name()}")
else:
    print(f"'{child_body_in_subtree.name()}' is the root or not in '{my_subtree.name()}'")

  3. Get the List of Base Bodies:

    • The baseBodies method returns a vector (or list) of body base class representing the base bodies of the subtree.

    • Recipe Step:

      base_bodies_list = my_subtree.baseBodies()
print(f"Base bodies of '{my_subtree.name()}': {[b.name() for b in base_bodies_list]}")

  4. Display the SubTree Structure:

    • The dumpTree method can be used to display the body tree, with options to control content and verbosity using dumpTreeOptions.

    • Recipe Step:

      # dumpTreeOptions can be configured for more detailed output
# e.g., options = Karana.Dynamics.SubTree.dumpTreeOptions(show_hinges=True, show_nodes=True)
my_subtree.dumpTree(prefix="  ", options=Karana.Dynamics.SubTree.dumpTreeOptions())

  5. Get the Type String:

    • The typeString method returns the string “Karana::Dynamics::SubTree”.

    • Recipe Step:

      print(f"Type of object: {my_subtree.typeString()}") # Output: Karana::Dynamics::SubTree

Setting the state of a sub-tree#

The state of a sub-tree consists of the Q and U generalized configuration and velocity coordinates for the subhinges and deformable bodies in the system. The kinematics and dynamics of multibody systems are fundamentally nonlinear and hence computations depend on the state values. Thus it is critical that the state values are set correctly before carrying out computations. There are multiple ways and considerations for getting and setting the state values as described below.

  • The state of an individual subhinge sh instance can be set using the sh.setQ() and sh.setU() methods. The sh.getQ() and sh.getU() methods can be used to query the current subhinge state values. This is usually the most intuitive and robust way of setting articulation state values, since the step involves getting access to the relevant subhinge and working with it directly (and no non-robust indexing and slicing operations are involved).

  • Deformable bodies have deformation state coordinates. The state of an individual deformable body bd instance can be set using the bd.setQ() and bd.setU() methods. The bf.getQ() and bd.getU() methods can be used to query the current body state values. This direct approach is again more robust since no indexing and slicing operations are involved.

Indirect Subhinge State initialization#

While a subhinge’s setQ() and setU() method can be used to directly set the state of a subhinge, sometimes the coordinate values to be set are not known explicitly, and are instead required to meet a specific transform relationship between a pair of frames.

One situation is where the subhinges coordinates are required to meet a relative transform T across the parent hinge. The subhinge coordinates can be set by calling hinge.fitQ(T). This method will do a best fit to set the hinge’s subhinge coordinates that achieve the desired T transform. The method will return a residual error transform - which should be identity if the desired T transform was achieved for the hinge.

There are times where the desired relative transform T across the hinge is not known directly either, and instead the requirement is for the subhinge coordinates to be such as to meet a desired relative pose across another pair of frames. For example, the coordinates of a vehicle’s hinge are to be initialized such that a frame attached to the vehicle has a specific relative pose with respect to another frame in the system. To make this concrete, let us assume that we have a desired T_AB pose that needs to be achieved between a pair of A and B frames by setting the coordinates of a hg hinge frame-to-frame on the path between A and B. To achieve this, call hg.fitQ(T_AB, f2f_AB). The second f2f_AB argument tells the method to interpret the first T_AB argument as a desired transform for the specified frame to frame instance, instead of the hinge itself. Like before, the method will do a best fit to set the hinge’s subhinge coordinates that achieve the desired T_AB transform. The method will return a residual error transform that should be identity if the requirement was fully met.

The similar fitU() and fitUdot() methods can be used to initialize the velocity and acceleration coordinates for the subhinges as well. Note that it is essential that the the Q coordinates be initialized correctly before using this procedure to initialize the U velocity coordinates, and the U coordinates in turn be initialized correctly before attempting to initialize the Udot acceleration coordinates.

Working with state coordinates in bulk#

The above methods directly work with the objects involved and are thus more intuitive and robust way of setting articulation and deformation state values. However they do require getting access to the relevant subhinge and body first before getting or setting its state. Their use is ideal for KModel instances (eg. motors, spring/dampers) that are associated with individual subhinges.

There are times however, where we may need to get and set the state in bulk across all the subhinges and deformable bodies. We can use the coordinates data members used to manage parts of the state.

  • Get the desired coordinates data cd instance from the sub-tree. The one for subhinges can be obtained from st sub-tree via st.subhingeCoordData(), for deformable bodies via st.bodyCoordData(). Additionally, if the sub-tree is actually a sub-graph, the coordinates data for the loop constraint hinges can be accessed via st.constraintCoordData().

  • The cd.getQ() and cd.getU() methods can be used to get overall Q and U coordinate vectors with contributions from all the components within cd. The cd.setQ(Q) and cd.setU(U) methods conversely can be used to set values across all the components. Assembling and inspecting the contents of the Q and U vectors does require understanding of the packing order of the component contributions. Due to this, manipulating the contents of these vectors directly can be source of error, and the earlier component level methods are recommended when such manipulation is necessary. The following methods are however are available when the packing information is needed:

    • the cd.coordOffsets(elt) method can be used to return the Q and U offsets for the state contribution of the elt component.

    • conversely, the cd.coordAt(index) method can be used to query for the component whose state is located at the index value.

  • The short-cut cd.setQ(scalar_Q) and cd.setU(scalar_U) methods can be used to fill all the values of the component states with specified scalar values. These methods are convenient for use during initialization.

Instead of working with the component coordinates data values, there is also the option to get and state values across all the sub-tree coordinates data and their components.

  • The st.getQ() and st.getU() methods will return Q and U vectors with state data from all the subhinge, deformable bodies and hinge loop constraints stacked together.

  • The corresponding st.setQ(Q) and st.setU(U) methods can be used to set the state data for all the subhinge, deformable bodies and hinge loop constraints in one-shot.

  • Similar to the coordinates data versions of the methods. the st.coordOffsets() and st.coordat() methods can be used to query the packing offset of a component object as well as the object at an index respectively.

For sub-graphics with constraints, the coordinates are not all independent. Additional steps such as described in Pose-level constraint kinematics section may be needed to ensure (or solve for) valid coordinate values that satisfy the constraints.

Using SubTree for Targeted Operations#

The primary benefit of subtrees is to apply computations to a limited scope. Many Karana dynamics algorithms are designed to accept a subtree (or subgraph) as an argument. Thus assuming a hypothetical ‘compute_dynamics_on_subtree()’ function, this function would take a SubTree argument and perform computations only on the sub-tree’s bodies.

Since Karana.Dynamics.Multibody is derived from Karana.Dynamics.SubTree, you can pass a full multibody instance to these algorithms if you want to perform computations on the entire system.

Computing mass properties of a SubTree#

To compute the mass properties of a subtree, you primarily use static methods available within the Karana.Dynamics.Algorithms namespace. These methods allow you to calculate various dynamic and kinematic properties, including overall mass matrices, momentum, and spatial inertia for the specified subtree argument.

One benefit of this capability is that the user can narrow the focus to just a part of the overall multibody system by creating a subtree for that part and passing it as an argument to these methods. Thus subtrees instances for individual vehicles, robot arms etc. can be created and used to limit the mass properties computation to just the bodies in the subtree.

Of course, the multibody instance is a also a subtree, and system-level mass-properties can by computed by passing the multibody as an argument to the methods. he following assumes that a my_subtree subtree instance already exists. If not use the steps in the Creating a SubTree Instance section to create one first.

I. Accessing the Center of Mass (CM) Frame

A Karana.Dynamics.SubTree instance has a dedicated frame that automatically tracks the center of mass (CM) location of its bodies. This frame provides a convenient way to track the overall center of mass of the subtree.

  • Purpose: Get the Karana.Frame.Frame instance that tracks the center of mass location of the bodies in the subtree.

  • Method: Karana.Dynamics.SubTree.cmFrame()

  • Recipe Step:

    cm_frame = my_subtree.cmFrame()
# You can then query properties of this frame, e.g., its translation relative to a parent frame
# print(f"Center of Mass Frame: {cm_frame.translation()}")

    • Insight: Since this is a Karana.Frame.Frame instance, it is integrated into the system’s frames tree, allowing its location to be tracked dynamically.

II. Computing Specific Mass Properties

The following static methods from Karana.Dynamics.Algorithms are used to compute various mass-related properties:

  1. Overall Spatial Inertia:

    • Purpose: Calculates the total spatial inertia of all bodies within the subtree.

    • Method: Karana.Dynamics.Algorithms.evalSpatialInertia()

    • Recipe Step:

      # Assuming 'my_subtree' is your Karana.Dynamics.SubTree instance
overall_spatial_inertia = Karana.Dynamics.Algorithms.evalSpatialInertia(my_subtree)
print(f"Overall Spatial Inertia:\n{overall_spatial_inertia}")

    • Insight: Passing a full multibody instance to this method will compute the system-level spatial inertia, while passing a strict subtree will compute it only for the bodies within that sub-tree.

  2. Overall Spatial Momentum:

    • Purpose: Calculates the overall spatial momentum for the subtree.

    • Method: Karana.Dynamics.Algorithms.evalSpatialMomentum()

    • Recipe Step:

      overall_spatial_momentum = Karana.Dynamics.Algorithms.evalSpatialMomentum(my_subtree)
print(f"Overall Spatial Momentum:\n{overall_spatial_momentum}")

  3. Centroidal Spatial Momentum:

    • Purpose: Calculates the centroidal spatial momentum for the subtree. This is the spatial momentum expressed at the subtree’s center of mass.

    • Method: Karana.Dynamics.Algorithms.evalCentroidalMomentum()

    • Recipe Step:

      centroidal_spatial_momentum = Karana.Dynamics.Algorithms.evalCentroidalMomentum(my_subtree)
print(f"Centroidal Spatial Momentum:\n{centroidal_spatial_momentum}")

  4. Centroidal Momentum Matrix:

    • Purpose: Calculates the centroidal momentum matrix for the subtree. This matrix relates the generalized velocities of the subtree to its centroidal momentum.

    • Method: Karana.Dynamics.Algorithms.evalCentroidalMomentumMatrix()

    • Recipe Step:

      centroidal_momentum_matrix = Karana.Dynamics.Algorithms.evalCentroidalMomentumMatrix(my_subtree)
print(f"Centroidal Momentum Matrix:\n{centroidal_momentum_matrix}")

  5. Tree System Mass Matrix (using Composite Rigid Body - CRB - approach):

    • Purpose: Calculates the tree system mass matrix using the Composite Rigid Body (CRB) recursive method. This mass matrix can have a customizable row/column order.

    • Method: Karana.Dynamics.Algorithms.evalTreeMassMatrixCRB()

    • Recipe Step:

      mass_matrix_crb = Karana.Dynamics.Algorithms.evalTreeMassMatrixCRB(my_subtree)
print(f"Tree Mass Matrix (CRB):\n{mass_matrix_crb}")

    • Note: This method currently does not support deformable bodies.

  6. Tree System Mass Matrix (using Newton-Euler inverse dynamics):

    • Purpose: Calculates the tree system mass matrix using a Newton-Euler inverse dynamics approach.

    • Method: Karana.Dynamics.Algorithms.evalTreeMassMatrixInvDyn()

    • Recipe Step:

      # 'cd' (CoordData) is optional; if provided, it specifies the order of rows/columns.
mass_matrix_inv_dyn = Karana.Dynamics.Algorithms.evalTreeMassMatrixInvDyn(my_subtree)
print(f"Tree Mass Matrix (Inverse Dynamics):\n{mass_matrix_inv_dyn}")

  7. Tree System Mass Matrix Inverse (using forward dynamics):

    • Purpose: Calculates the inverse of the tree system mass matrix using a forward dynamics approach.

    • Method: Karana.Dynamics.Algorithms.evalTreeMassMatrixInvFwdDyn()

    • Recipe Step:

      # 'coord_data' is required and specifies the order of rows/columns for the inverse matrix.
# Assuming 'my_coord_data' is a `CoordData` instance
# mass_matrix_inv_fwd_dyn = Karana.Dynamics.Algorithms.evalTreeMassMatrixInvFwdDyn(my_subtree, my_coord_data)
# print(f"Tree Mass Matrix Inverse (Forward Dynamics):\n{mass_matrix_inv_fwd_dyn}")

By following these steps, you can comprehensively compute and access the various mass-related properties of any subtree within your multibody system.

Working with sub-graphs#

Karana.Dynamics.SubGraph is another related class that inherits from subtree, designed for portions of multibody systems that might include loop constraints.

The steps for creating a sub-graph instance are similar to those for a sub-tree described in the Working with sub-trees section, except that the the SubGraph::create() should be used. Once the sub-graph has been created, existing or new constraints can be added to it using the enableLoopConstraint() and enableCoordinateConstraint() methods.

Constraint kinematics solver#

The Karana.Dynamics.ConstraintKinematicsSolver class is designed to find generalized coordinates (Q), velocities (U), and accelerations (Udot) coordinates that are consistent with the closure constraints present in a subgraph.

The solver works by iteratively adjusting the system’s state until the constraint equations are satisfied, leveraging Jacobian matrices and a nonlinear solver. The method takes into into account the closure constraints that have been enabled for the subgraph.

I. Understand the Problem Domain

  • Purpose: To resolve kinematic inconsistencies introduced by closure constraints in multibody systems.* Input: An initial guess for the system’s generalized coordinates, velocities, and accelerations.

  • Output: Updated generalized coordinates (Q), velocities (U), and accelerations (Udot) that satisfy all defined loop constraints.

A subgraph instance with one or closure loop constraints is used in the following procedure.

  1. Create the solver: Create an Instance of constraint kinematics solver by calling Karana.Dynamics.Algorithms.constraintKinematicsSolver() method with your subgraph as argument.

  2. Frozen Coordinates: At this time, you can freeze coordinates using the Karana.Dynamics.ConstraintKinematicsSolver.freezeCoord() method. Coordinates for the frozen coordinates are left unchanged by the solvers invoked in the later steps. This is useful for managing redundant coordinates or solving around a preferred configuration. Coordinates can be unfrozen using the Karana.Dynamics.ConstraintKinematicsSolver.unfreezeCoord() method.

  3. Initial Guess for State: Since the solver uses iterative methods, it requires an initial “guess” for the generalized coordinates (Q), velocities (U), and accelerations (Udot) of the system. This guess is set on the multibody instance itself. Provide an Initial Guess for the System State by setting the initial generalized coordinates (Q), velocities (U), and accelerations (Udot) on the subgraph.

  4. Solve for Consistent Coordinates: (Q)

  5. Solve for Consistent Velocity Coordinates: (U)

    • The Karana.Dynamics.ConstraintKinematicsSolver.solveU() method solves for U velocity coordinates for the subgraph that satisfy the velocity-level constraints. Note that this method should only be called after Q values satisfying the constraints have been set in the subgraph. The solution can be retrieved using the subgraph’s getU method. The method returns the residual error - which should be essentially zero when a solution is found.

  6. Solve for Consistent Acceleration Coordinates: (Udot)

    • The Karana.Dynamics.ConstraintKinematicsSolver.solveUdot() method solves for Udot acceleration coordinates for the subgraph that satisfy the acceleration-level constraints. Note that this method should only be called after Q and U values satisfying the constraints have been set in the subgraph. The solution can be retrieved using the subgraph’s getUdot() method. The method returns the residual error - which should be essentially zero when a solution is found.

Forward dynamics/Solving Equations of Motion#

In kdFlex, computing forward dynamics involves determining the accelerations of a multibody system given its current state (positions and velocities) and applied forces/torques. This is a core operation for simulating how a system moves over time. kdFlex provides multiple algorithms for forward dynamics, catering to different system complexities (tree vs. graph topologies, presence of loop constraints).

The forward dynamics algorithms in kdFlex typically take as input:

  • The multibody system’s current configuration (joint positions Q and velocities U).

  • Applied generalized forces/torques (T).

  • And they output: Generalized accelerations (Udot).

The framework offers both explicit algorithm calls and an integrated state propagator for time-domain simulations that internally use forward dynamics.

The Karana.Dynamics.Algorithms namespace provides static methods for directly evaluating forward dynamics.

  1. For SubTree Systems (no closure constraints):

    • Algorithm: Articulated Body Inertia (ATBI) based forward dynamics.

    • Method: Karana.Dynamics.Algorithms.evalTreeForwardDynamics()

    • Input: A Karana.Dynamics.SubTree instance (a multibody instance can also be passed, as it derives from subtree).

    • Recipe Step:

      # Assuming 'mb' is your Karana.Dynamics.Multibody instance
# and its state (q, u) and applied forces are set.
# If you want to compute for a sub-tree, create one first:
# my_subtree = Karana.Dynamics.SubTree.create("my_sub", mb.getBody("some_body_name"))

Karana.Dynamics.Algorithms.evalTreeForwardDynamics(mb) # or evalTreeForwardDynamics(my_subtree)
# After this call, the accelerations (udot) for the system/subtree are computed
# and stored internally or can be queried from the system's state.

    • Note: This method evaluates the ATBI-based forward dynamics.

  2. For SubGraph Systems (with closure constraints):

    • When subgraph has closure constraints, you need specialized algorithms. It is important that the Q and U state vectors be first initialized to values that are consistent with the closure constraints on the system. These methods operate on a Karana.Dynamics.SubGraph instance.

    • a. Tree-Augmented (TA) based exact forward dynamics: This method enforces the bilateral constraints exactly.

          # Assuming 'sg' is your Karana.Dynamics.SubGraph instance with loop constraints
    # and its state (q, u) and applied forces are set.

    Karana.Dynamics.Algorithms.evalTAForwardDynamics(sg)
    # Accelerations (udot) are computed for the subgraph.

    • b. Baumgarte method for forward dynamics: This method enforces the bilateral constraints only approximately - and its use is generally not recommended.

      • Method: Karana.Dynamics.Algorithms.evalBaumgarteForwardDynamics()

      • Input: A Karana.Dynamics.SubGraph instance, along with stiffness and damping parameters for the Baumgarte stabilization method.

      • Recipe Step:

       # Assuming 'sg' is your Karana.Dynamics.SubGraph instance
 # and its state (q, u) and applied forces are set.
 stiffness_val = 100.0 # Example value
 damping_val = 10.0    # Example value

 Karana.Dynamics.Algorithms.evalBaumgarteForwardDynamics(sg, stiffness_val, damping_val)
 # Accelerations (udot) are computed for the subgraph using the Baumgarte method.

The forward dynamics algorithms in kdFlex are in fact hybrid algorithms, that support prescribed motion at the subhinges. This means that the O(N) recursive hybrid dynamics algorithm can reduce to standard forward dynamics (or inverse dynamics) depending on the inputs (mix of generalized forces and prescribed accelerations).

Computing inter-body forces#

In kdFlex, computing inter-body forces refers to finding the forces and torques acting across the hinges (joints) that connect bodies in a multibody system. These forces are internal to the system and are crucial for understanding the loads on joints and for design analysis.

I. Compute Forward Dynamics: Inter-body forces are typically a result of (or can be computed after) a forward dynamics calculation, as they depend on the accelerations and inertia of the connected bodies. The sources suggest that methods for retrieving inter-body forces should only be called after forward dynamics has been called. However the internal _enable_interbody_force flag within the physical hingeInterBodyForce struct must be set before the forward dynamics computation is done.

II. Enabling and Accessing Inter-Body Forces

kdFlex includes mechanisms to enable and store inter-body forces.

kdFlex provides an inexpensive expression for computing inter-body forces using the Spatial Operator Algebra (SOA) algorithm even though the standard ABI forward dynamics doesn’t require it. This means the computation is efficient but might be an optional step during dynamics if not explicitly needed for the primary forward dynamics calculation. It also offers specific methods to retrieve these forces.

            # Assuming 'mb' is your Karana.Dynamics.Multibody instance
            # and 'my_hinge' is a {{ hingecls }} object in your system.
            # First, ensure forward dynamics has been computed:
            Karana.Dynamics.Algorithms.evalTreeForwardDynamics(mb)

           # Access the hinge's onode and then its interBodyForce
           # Note: Accessing HingeOnode directly might require delving into the internal structure
           # A more direct way would be to get the hinge object itself.
           # Let's assume 'a_physical_hinge' is an instance of {{ hingecls }}
           # or you can retrieve it from a body:
           # body_B = mb.getBody("body_B")
           # a_physical_hinge = body_B.parentHinge() # Get the hinge connecting body_B to its parent

           # If 'a_physical_hinge' is available:
           # inter_body_force_vector = a_physical_hinge.interBodyForce() # This might be the direct call in Python binding
           # print(f"Inter-body force for hinge: {inter_body_force_vector}")

  • Note: The interBodyForce() method returns a Karana.Math.SpatialVector, which represents both linear force and angular torque.

  • For SubGraph systems with constraints:

    • When dealing with loop constraints (which often involve “cut-joint hinges” conceptually), you use methods specifically designed for Tree-Augmented (TA) dynamics.

    • Method (from hinge loop constraint): Karana.Dynamics.LoopConstraintHinge.getTAInterBodyForce()

      • Purpose: Returns the inter-body spatial force at the source node for TA dynamics.

      • Recipe Step:

              # Assuming 'my_loop_constraint_hinge' is an instance of LoopConstraintHinge
        # and TA forward dynamics has been computed on the SubGraph containing it.
        # Example:
        # my_subgraph = Karana.Dynamics.SubGraph(...)
        # Karana.Dynamics.Algorithms.evalTAForwardDynamics(my_subgraph)

        # ta_inter_body_force = my_loop_constraint_hinge.getTAInterBodyForce()
        # print(f"TA Inter-body force for loop constraint: {ta_inter_body_force}")

      • Insight: This force is at the source node and has a sign as if acting on the target node. It includes contributions from generalized forces at the cut-joint. This method should only be called after TA forward dynamics has been executed.

Flexible body dynamics#

kdFlex flexible body definition (manual)#

Creating a a serial-chain system with flexible bodies follows the same process as in the Create rigid body serial-chain multibody system (manual) recipe, except that in we call the static Karana.Dynamics.PhysicalModalBody.create() method, with params of type Karana.Dynamics.PhysicalModalBodyParams with additional deformation related parameters for the flexible bodies.

Procedurally create flexible body serial chain#

Creating a a serial-chain system with flexible bodies follows the same process as in the Procedurally create rigid body serial chain recipe, except that in Step 2 we call the static Karana.Dynamics.PhysicalModalBody.addSerialChain() method, with parameters of type Karana.Dynamics.PhysicalModalBodyParams with additional deformation related fields for flexible bodies.

Procedurally create flexible body tree#

Procedurally creating a tree of physical modal bodies follows the process in Procedurally create a regular rigid body tree section except the that the Karana.Dynamics.PhysicalBody.addTree() call is replaced with the Karana.Dynamics.PhysicalModalBody.addTree(), with with parameters of type Karana.Dynamics.PhysicalModalBodyParams with additional deformation related fields for flexible bodies.

Linearized state space model#

Multibody dynamics models are nonlinear. Linearized, input.output state-space models are however useful for control system design and analysis. The steps for creating such linearized models are:

  • define a u_fn(x, u) input callback function that will use a x state vector and an input u vector to compute and set appropriate forces, gravity etc values in the multibody model to effect the multibody state-derivative computation.

  • define a yu_fn(x, u) output callback function that will compute the output as a function of the x state vector and the input u vector.

  • Call the Karana.Dynamics.Algorithms::stateSpaceGenerator(sg, n_inputs, n_outputs, u_fn, y_fn) method to compute and return a StateSpace generator instance ss_gen for creating linearized, (A,B,C,D) state space representation of the system about the current state with inputs and outputs defined by the vector-values u_fn and y_fn functions.

  • Set the system state Q and U values, and call ss_rep = ss_gen.generate() to generate an ss instance with linearized (A,B,C,D) state space matrix values about this configuration. The method can be called multiple times for different system state values to compute state-space representations about different configurations for the system.

The generality of this approach allows the users flexibility in making simple to complex choices for inputs and outputs. The above process applies to systems with rigid and deformable bodies.

Kinematics#

Relative pose, velocities, accelerations#

To query the relative pose, velocity or acceleration of a pair of frames A and B

  • create a frame-to-frame instance using A2B_f2f = A.frameToFrame(B)

  • the relative pose of A to B can then be obtained as a homogeneous transform using T = A2B_f2f.relTransform()

  • the relative spatial velocity of B with respect to A (as observed and represented in the A frame) can then be obtained via V = A2B_f2f.relSpVel()

  • the relative spatial acceleration of B with respect to A (as observed and represented in the A frame) can then be obtained via A = A2B_f2f.relSpAccel()

Since physical body and node instances are frame instances, they can be used as A and B above in the above expressions for arbitrary queries between bodies, nodes and frames. Moreover, subhinges and hinges are frame-to-frame instances, so their instance can be directly queried for such relative information.

End-effector Jacobian#

The Jacobian defines the (coordinate-dependent) matrix that maps the generalize velocity coordinates to a relative spatial velocity. The first step thus is to identify a frame-to-frame instance, say f2f whose relSpVel() we are interested in obtaining a Jacobian for. Towards this

  • first create a jacobian generator using jac_gen = Algorithms.jacobianGenerator([f2f], cd) for this f2f with the
    coordinates data whose components to generate the Jacobian for.

  • then call my_jac = jacgen.jacobian() to obtain the Jacobian matrix. The same generator can be used repeatedly as the coordinates change to obtain the updates Jacobian matrix values. The two step approach is used to avoid repeating the setup costs for each invocation of the jacobian() method.

Note that this Jacobian capability is quite general. To obtain an end-effector Jacobian, we simply need to create a frame-to-frame instance from the root frame to the end-effector frame, and use the above recipe. This procedure can be used for Jacobians for the relative spatial velocity between moving frames, such as between end-effectors of multi-limbed robotic systems. All that is required is the frame-to-frame specifying the frame pair of interest.

Multi-arm Jacobian#

There are times when we want to track multiple Jacobians at once, such as during multi-fingered grasps. One option would be create jacobian generator instances - one each for each desired Jacobian. A more efficient way is to create a multi-jacobian generator as follows:

  • create a list of all the frame-to-frame instances that identify the list of f2f instances for the desired Jacobians

  • Create multi-jacobian generator using the mjac_gen = Algorithms.jacobianGenerator(f2fs, cd) call with the list of f2f instances

  • call the mjac_gen.jacobian() method to get the overall Jacobian with block Jacobian entries for each of the f2f instances.

Once again, the multi-Jacobian generator can be used over and over as the coordinates change to get the updated Jacobian values.

Pose-level constraint kinematics#

Systems with bilateral constraints have fewer degrees of freedom than defined by the hinges in the underlying multibody tree. This brings up the need to answer questions such as the actual degrees of freedom, the best sub-set to use as independent generalized velocity coordinates, the mapping between independent and dependent etc. Note that the answers to these questions is coordinate dependent. The following steps show how these questions can be answered at the pose-level for a subgraph sg with internal bilateral constraints.

  • Pose constraint errors: To check whether the current tree hinge+body Q generalized coordinates satisfy the bilateral constraints, call pose_err = sg.cks().poseResiduals(). The returned value should be zero if the constraints are satisfied at the pose level. This method does not however tell us anything about the constraint methods.

    To check whether the current tree hinge+body and the constraint hinge Q generalized coordinates satisfy the bilateral constraints, call pose_err = sg.poseError(). The returned value should be zero if the constraints are satisfied at the pose level.

  • Solving for Q: if the pose error is non-zero, we can use the current coordinates to solve for a set of subhinge+body+constraint coordinates that satisfy the pose-level constraints by calling err = sg.cks().solveQ(). A nonlinear solver is used to iteratively find Q coordinates that satisfy the constraints. Success is indicated by a zero err returned value. The coordinates can be obtained by calling sg.getQ().

  • Freezing coordinates: When the system is under-constrained, that is, there are more degrees of freedom than necessary to satisfy the constraints, there is the opportunity to only use a subset of the coordinates to satisfy the constraints. The remaining coordinates can then be used to meet other objectives. Such a need can be met by freezing individual coordinates for the purposes of the solution obtained by the solveQ() method. Each individual coordinate is associated with a subhinge coordinate. The sg.cks().freezeCoordinate(shg, index) call can be used multiple times to freeze a set of coordinates. Subsequent solveQ() calls will leave the values of these coordinates unchanged. Coordinates can be unfrozen using the unfreezeCoordinate() method. One caution to keep in mind is that solveQ() may fail to find a solution if too many coordinates have been frozen.

Velocity-level constraint kinematics#

Bilateral constraints also limit the admissible velocity motion. Before meaningfully tackling velocity level constraints, it is essential that the system is in in a configuration where the pose-level constraints have been satisfied as described in the Pose-level constraint kinematics section.

  • Velocity constraint errors: To check whether the current set of U generalized velocity coordinates satisfy the constraints, the sg.velError() method can be used to check whether the returned error vector is zero or not.

  • Velocity constraint matrix Gc: A vector U of generalized velocities satisfies the bilateral constraints if Gc * U = 0 where Gc denotes the velocity constraint matrix. This configuration dependent matrix can be obtained using Gc = Algorithms.evalVelocityConstraintMatrix(SubGraph(sg, with_constraints). The with_constraints boolean argument should be set to True to include rows for the constraint hinge coordinates in the output.

  • Solving for U: If the current velocity coordinates do not satisfy the constraints, the err = sg.cks().solveU() method can be used to solve for a U that satisfies the constraints. The returned error vector will be zero if a solution was obtained. The solution will not change the values of the frozen coordinates. We again emphasize that the resulting solution is valid, only if the system configuration is such that the pose-level constraints have already been satisfied.

  • Independent constraints: For constrained systems, and important question is as to how many of the constraints are independent. The number of independent constraint is the row-rank of the Gc velocity constraint matrix. The Algorithms.evalIndepConstraintCoordIndices(sg) method can be used to select the best subset of independent constraints at the current configuration. Note that the best set is configuration dependent.

    Many dynamics computations assume require that the velocity constraint matrix be full row-rank. To meet this need, and once the independent set of constraints has been identified, a modified, full row-rank velocity constraint matrix may be used by only using the corresponding rows of the original Gc matrix to define the new velocity constraint matrix.

    An additional serious challenges with constrained dynamics is that the number of independent constrain may vary with the system configuration as well. The methods described above can be used to monitor the constraints during the course of simulations to make appropriate adjustments as needed.

Coordinate-partitioning#

  • Independent coordinates: In light of the dependency among the velocity coordinates, the coordinate-partitioning approach seeks to partition the U velocity coordinates into independent and dependent coordinates. the Algorithms.evalCoordinatePartioning(sg) method returns the best choice independent coordinates at a specific configuration. Note that the choice may vary with the system configuration. Due to this variability, users may choose to work with a different choice of independent velocity coordinates (same number however) that work across a broader range of configurations.

  • Dependency map: Once a independent set of coordinates has been chosen, it is possible to obtain the dependent coordinates such that the overall U coordinates satisfy the constraints. The dependent coordinates mapping matrix D such that U_dependent = D * U_independent can be obtained using D = Algorithms.evalDependentCoordinatesMatrix(indep_indices, full) where indep_indices is the list of coordinates to be treated as independent (length equal to the row-rank of Gc). The full argument should be set to True to include rows for the independent coordinates in the output matrix. The D matrix is coordinate dependent.

  • Locked coordinates: Due to the coupling between constraints, it can happen that some of the velocity coordinates are locked. These coordinates can be identified by looking for all-zero rows of the D dependency matrix.

Acceleration-level constraint kinematics#

Bilateral constraints also limit the admissible acceleration motion. Before meaningfully tackling acceleration level constraints, it is essential that the system is in in a configuration where both the pose and velocity-level constraints have been satisfied as described in the Pose-level constraint kinematics and a Velocity-level constraint kinematics section.

  • Acceleration constraint errors: To check whether the current set of Udot generalized acceleration coordinates satisfy the constraints, the sg.cks().accelError() or sg.cks().accelResidual() methods can be used to check whether the returned error vector is zero or not.

  • Solving for Udot: If the current acceleration coordinates do not satisfy the constraints, the err = sg.cks().solveUdot() method can be used to solve for a Udot that satisfies the constraints. The returned error vector will be zero if a solution was obtained. The solution will not change the values of the frozen coordinates. We again emphasize that the resulting solution is valid, only if the system configuration is such that the pose and velocity-level constraints have already been satisfied.

Inverse kinematics#

The inverse kinematics (IK) problem requires solving for the Q configuration coordinates that set the pose for one or more frames on the multibody system to desired values. In the simplest form it may be used for positioning a robot arm end-effector at a specific location, while more generally it may simultaneously position multiple such end-effectors for a multi-limbed system.

The IK problem is solved in kdFlex as a special case of the general support for pose constrained in multibody systems described in Pose-level constraint kinematics section. The general idea is to first

  • create a dedicated subgraph sg for IK purpose

  • create and add hinge constraint (and associated frames) for the system frames whose IK pose solution is desired. Hinge constraints of the locked type should be used wherever the full pose of a system frame needs to set.

  • freeze the coordinates for the hinge constraints so that they will remain untouched by the solution process

Then to obtain the IK solution

  • Relocate the constraint frames to the desired location for each of the corresponding frames.

  • call sg.solveQ() to solve for the Q coordinates. On success, the Q coordinates will satisfy the IK constraints, and thus position the system frames at the constraint frames.

  • these two steps can be repeated over and over as the IK positioning needs evolve for the system.

This approach allows IK solutions involving multiple frames to be positioned. Furthermore, by using appropriate constraints, the relative pose requirements can be across non-stationary frame pairs that are both attached to the multibody system. Furthermore, by using appropriate loop hinge constraints, the IK solution can allow for finding solutions where full to partial pose of the system frames to be set. For instance, if the requirement is to only position the system frame at a location with no restrictions on orientation, a hinge constraint of Karana.Dynamics.HingeType.BALL type should be used. For the case where orientation only needs to be set, a Karana.Dynamics.HingeType.TRANSLATIONAL hinge type should be used. for full pose control, a Karana.Dynamics.HingeType.LOCKED type hinge should be used.

State Propagation#

The Karana::Dynamics::StatePropagator class:

  • Manages the simulation time.

  • Holds the current state vector (generalized positions and velocities).

  • Uses an underlying numerical integrator (e.g., RK4, CVODE) to solve the ordinary differential equations (ODEs) that describe your multibody system’s dynamics.

  • Allows for registering events that trigger at specific times or periodically.

Creating a state propagator#

Creating a Karana::Dynamics::StatePropagator is a crucial step for performing time-domain simulations of your multibody system. The StatePropagator is responsible for integrating the system’s equations of motion over time, advancing its state (generalized positions Q and velocities U).

You must have a subtree or subgraph instance that represents your mechanical system. The StatePropagator operates on this system.

  • This should be fully defined with bodies, hinges, and any other relevant components.

  • It’s good practice to call ensureCurrent() on it before passing it to the StatePropagator.

The following method can be used to create a state propagator. You provide your subtree object and specify the integrator_type.

  • Method Signature: StatePropagator::create(kc::ks_ptr<SubTree> st, km::IntegratorType integrator_type, ...)

  • Parameters:

    • st: The subtree instance.

    • integrator_type: an enum specifying the numerical integrator to use. Common choices are:

      • CVODE_STIFF: For stiff systems (often recommended for robustness).

      • RK4: A simpler, fixed-step Runge-Kutta 4th order integrator (good for non-stiff systems or when fixed steps are required).

    • solver_type: An enum specifying the type of multibody dynamics solver to use. The option for subtree systems is TREE_DYNAMICS. For a subgraph, the best option is TREE_AUGMENTED_DYNAMICS.

  • Recipe Step:

        # Create the StatePropagator
        # Using "cvode_stiff" for general robustness, or "rk4" for simplicity/fixed steps
        integrator_type = CVODE_STIFF
        solver_type = TREE_DYNAMICS
        sp = kd.StatePropagator(mb, integrator_type, None, None, solver_type)
        print(f"StatePropagator created using '{integrator_type}' integrator.")

Registering and unregistring a KModel#

By convention, KModels are registered with a state propagator at construction. Therefore, they typically only need to be registered if they were previously manually unregistered. A KModel can be registered manually by using the StatePropagator.registerModel method and unregistered manually by using the StatePropagator.unregisterModel method.

KModel param initialization#

By convention, KModel’s parameters start off uninitialized and must be initialized by the user. How this is done is model/parameter dependent. The parameters are always located on the params member of the KModel, and are typically set either via member variables on params or via setter methods. See the Model parameters section for more details.

Setting state propagator state#

The state propagator initial time and continuous states for the numerical integrator must be initialized before running a simulation. This can be done by calling the StatePropagator.setTime(t) method to set the t initial time, and the StatePropagator.setState(x) method to set the x initial continuous state values. The x state vector combines the multibody system’s Q and U coordinates together with the continuous state coordinates of all of the registered models. Assembling the x vector manually is not recommended since there can be errors in the packing order. The recommended steps instead are to:

  1. initialize the Q and U coordinates of all the physical subhinges objects in the multibody system by looking them up and calling their setQ() and setU() methods.

  2. initialize the continuous states for any registered KModel that has them.

  3. call the x = StatePropagator.assembleState() method to correctly combine the component coordinates set in steps (1) and (2) into an x state vector.

  4. call StatePropagator.setState(x) method to set the initial state in the state propagator.

The propagator’s setState() method can be called at any time to reset the integrator state - except when there have been configuration changes that may have changed the size or structure of the state. In this case a hard reset is required When there are constraints present, this method will verify that the new state satisfies the constrains and throw an error if that is not the case.

Data logging#

Data logging is done with the Karana.KUtils.H5Writer. This class takes uses Karana.KUtils.PacketTableConfig to define tables and functions that populate the column of said tables. These functions are used to calculate a new row each time H5Writer.log is called. They can be defined in C++ or Python. See the Setting up your data logger section for more details.

Defining tables#

Tables are defined using the Karana.KUtils.PacketTableConfig class. A summary of the steps follows. These steps are shown in detail in the Defining data with PacketTableConfig section.

  1. Use the create method to create a new PacketTableConfig.

  2. Add columns to the table using the addData methods.

  3. Add the table from PacketTableConfig to the H5Writer using the createTable method.

Using events#

Different types of events#

Broadly speaking, events can be broken down into three categories:

  1. Events that happen at a certain time(s)

  2. Events that happen at a fixed point when advancing the dynamics forward in time (like at every derivative call or at every hop)

  3. Events that are triggered based on the value of some variable (a zero-crossing event).

Events that happen at certain time(s) are created and added to the simulation via timed events. See the Timed events section for details.

Events that happen at a fixed point when advancing the dynamics forward in time are located on one of the following CallbackRegistrys. These are all located on the StatePropagator.fns.

  • pre_deriv_fns - Events on this CallbackRegistry will run before each derivative call.

  • post_deriv_fns - Events on this CallbackRegistry will run after each derivative call.

  • pre_hop_fns - Events on this CallbackRegistry will run before each simulation hop.

  • post_hop_fns - Events on this CallbackRegistry will run after each simulation hop. Events are registered with CallbackRegistrys using map- or dictionary-like access. For example, in C++

sp->fns.pre_deriv_fns["my_event"] = [](const km::Ktime& t){std::cout << t << std::endl;}

and in Python

sp.fns.pre_deriv_fns["my_event"] = lambda t: print(t)

Events that are triggered based on the value of some variable are called zero-crossing events. These are discussed in detail in the Zero-crossings section.To set one of these events, use the zero_crossing_fns CallbackRegistry from StatePropagator.fns. This event takes as input the current state vector and a boolean. The input boolean indicates whether the event has triggered or not, and it outputs a boolean that indicates when the zero has been crosed. Here is an example in C++

sp->fns.zero_crossing_fns["my_zero"] = [](const km::Vec &x, bool crossed) {
    if (crossed) {
        std::out << "Executing the zero crossing function!." << std::endl;
        // Do other work here
        return true;
    } else {
        return x[0] < 0.0;
    }
};

This will trigger once the first element of the state vector falls below zero. Here is the same function, but implemented in Python

def _zc(x: NDArray[np.float64], crossed: bool):
    if crossed:
        print("Executing the zero crossing function!.")
        # Do other work here
        return True
    else:
        return x[0] < 0.0
sp.fns.zero_crossing_fns["my_zero"] = _zc

Plotting#

Dynamic data plotting (plots that are updated as the simulation is running) can be done using the Karana.KUtils.DataPlotter.DashApp. This class is used to quickly create a Dash app whose data is updated dynamically as the simulation progresses. The Karana.KUtils.DataPlotter.PlotDataDS is used to define the data to be plotted. Here is a brief example:

def time():
    return ktimeToSeconds(sp.getTime())

def vecData():
    return myFrameToFrame.relSpVel().getv()

def scalarData():
    return np.linalg.norm(myFrameToFrame().relTransform().getTranslation())

pds = PlotDataDS(
    x_data_name="t", # Name of the independent variable
    x_data=time, # Function for the independent variable
    y_data={"s": scalarData, "v": vecData}, # Dictionary that defines dependent variables
    title="test1" # Title for the plot
)

d = DashApp([pds], title="Test Dash App")

In this example, a PlotDataDS is used to define the data to be plotted. The dependent variables can be scalar or vector values. Then, the DashApp is used to create a live plotting instance. This takes a list of PlotDataDS instances, so it can be used to show multiple plots on the same web frontend. Once the app is started, it will print a URL on the terminal that one can follow to view the plot(s).

To update the plots manually, simply call the update method on the DashApp. This will invoke all the functions in the PlotDataDS and use their result to update the plot data.

Oftentimes, in a simulation, one wants the update method to be called automatically as the simulation is stepping. This can be done via the Karana.Models.DataPlotter KModel.

It can be used like this:

dp = DataPlotter("plot_data", sp)
dp.setPeriod(0.1)
dp.params.dash_app = d

In this example, sp is a state propagator, and the update will be called by the model ever 0.1 seconds as the simulation progresses.

Graphics visualization#

The setupGraphics helper method in kdFlex is designed to streamline the process of setting up the graphics environment, including the scene and a web server for browser visualization. It’s typically called on a multibody (mb) object.

Here’s a recipe for setting up graphics using setupGraphics:

Call setupGraphics

Call the setupGraphics helper method on your multibody object. This method takes care of initiating the graphics environment for you.

cleanup_graphics, web_scene = multibody.setupGraphics(port=0, axes=0.5)

Explanation of arguments:

  • port (int, default 29523): Specifies the port to bind the WebUI server to. Using port=0 requests an arbitrary unused port, which is a common practice to avoid port conflicts.

  • axes (float, default 1.0): Determines the length of the axes visualization on the root frame of your scene. Examples show usage with 0.5 or 0.0.

  • client_type (Literal, default 'auto'): Can be 'auto', 'electron', 'notebook', or 'webbrowser', controlling how the client for the WebUI is launched.

  • origin_frame: An optional Frame object to center the client’s origin.

  • wait_for_clients (int, default 0): Specifies if the system should wait for a certain number of clients to connect before proceeding. Examples show wait_for_clients=1.

  • wait_for_clients_timeout (float, default 0.0): The timeout duration for waiting for clients.

Return Values:

The setupGraphics method returns two key arguments:

  1. cleanup_graphics: A callable function that you will use later to clean up the scene layer and web server when your simulation concludes.

  2. web_scene: A web scene object. This object provides an interface to programmatically interact with the browser visualization, such as setting the camera position and orientation. The WebScene class is a derived type of GraphicalScene, which is a scene with an added graphics-specific interface.

Stick figure graphics#

Adding “stick parts” in kdFlex is a straightforward way to visualize the links and structure of your multibody system. These stick parts are simple graphical representations that help you quickly understand the kinematic configuration of your model.

Here’s a recipe specifically for adding stick parts:

Ensure Graphics Setup is Complete

Before adding stick parts, you need to have initialized your graphics environment using setupGraphics. This provides the scene where the stick parts will be rendered.

# Assuming 'mb' is your initialized multibody object
cleanup_graphics, web_scene = mb.setupGraphics(port=0, axes=0.5)

Call createStickParts() on Your Multibody Object

The createStickParts() method is called directly on your multibody (mb) object. It automatically generates and attaches simple stick-like geometries to represent the links (rigid bodies) of your multibody system within the scene.

mb.createStickParts()

Optionally Customize with StickPartConfig

You can customize the geometry and appearance of the stick parts. For example:

from Karana.Dynamics import StickPartsConfig
stick_config = StickPartsConfig()
stick_config.body_stick_radius = 0.005
stick_config.pin_hinge_radius = 0.005
stick_config.sphere_hinge_radius = 0.005
stick_config.node_radius = 0.005
mb.createStickParts(stick_config)
Contents