Procedural n-link pendulum collision example

This notebook is a more advanced version of the n-link pendulum and 2-link pendulum collision tutorials.In this notebook a pendulum is created using the procedural approach to allow for an n number of linked bodies to be built at once. Collision detection is added in this notebook and visualized using material colors.

Requirements:

• n-link Pendulum

• 2-link Pendulum Collision

In this example we will:

• Create the n-link pendulum multibody

• Create the kdFlex Scene

• Setup geometries

• Set up the state propagator and models

• Set initial state

• Register a timed event

• Run the simulation

• Clean up the simulation

For a more in-depth descriptions of kdflex concepts see usage.

import numpy as np
from typing import cast
from math import pi
import atexit
from Karana.Math import IntegratorType
from Karana.Core import discard, BaseContainer, allFinalized
from Karana.Frame import FrameContainer
from Karana.Dynamics import (
    Multibody,
    PhysicalBody,
    PhysicalBody,
    HingeType,
    StatePropagator,
    TimedEvent,
    PhysicalBodyParams,
)
from Karana.Math import UnitQuaternion, SpatialInertia, HomTran
from Karana.Scene import (
    SphereGeometry,
    BoxGeometry,
    CapsuleGeometry,
    CylinderGeometry,
    Color,
    PhysicalMaterialInfo,
    PhysicalMaterial,
    ScenePartSpec,
    LAYER_COLLISION,
    LAYER_GRAPHICS,
    LAYER_ALL,
)
from Karana.Scene import ProxySceneNode, ProxyScenePart
from Karana.Scene import CoalScene
from Karana.Collision import FrameCollider, HuntCrossley
from Karana.Models import UniformGravity, UpdateProxyScene, SyncRealTime, PenaltyContact

Create the n-link Pendulum Multibody

See Multibody or Frames for more information relating to this step.

def createMbody(n_links: int):
    """Create the Multibody.

    Parameters
    ----------
    n_links : int
        Number of links in the pendulum system.

    Returns
    -------
    Multibody
        The newly created multibody.
    """
    fc = FrameContainer("root")
    mb = Multibody("mb", fc)

    # inertia used for both bodies
    spI = SpatialInertia(2.0, np.zeros(3), np.diag([3, 2, 1]))

    # inboard to joint transform used for both bodies
    ib2j = HomTran()

    # body to joint transform used for both bodies
    b2j = HomTran(np.array([0, 0, 1.0]))

    # adding colored material
    mat_info = PhysicalMaterialInfo()
    mat_info.color = Color.BLUE
    blue = PhysicalMaterial(mat_info)
    mat_info.color = Color.LIGHTBLUE
    light_blue = PhysicalMaterial(mat_info)

    # Add a scene part that is for collisions and
    sp_col = ScenePartSpec()
    sp_col.name = "sp_collision"
    sp_col.geometry = SphereGeometry(0.3)
    sp_col.material = blue
    sp_col.transform = HomTran([0.0, 0.0, 0.0])
    sp_col.scale = [1, 1, 1]
    # LAYER_ALL is default scenepart layer which this part belongs to

    # Add a scene part that is purely visual
    sp_vis = ScenePartSpec()
    sp_vis.name = "sp_graphics"
    sp_vis.geometry = CylinderGeometry(0.025, 1.0)
    sp_vis.material = light_blue
    sp_vis.transform = HomTran(q=UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]), vec=[0.0, 0.0, 0.5])
    sp_vis.scale = [1, 1, 1]
    sp_vis.layers = LAYER_GRAPHICS

    # params for each body in pendulum
    params = PhysicalBodyParams(
        spI=spI,
        axes=[np.array([0.0, 1.0, 0.0])],
        body_to_joint_transform=b2j,
        inb_to_joint_transform=ib2j,
        scene_part_specs=[sp_col, sp_vis],
    )

    PhysicalBody.addSerialChain(
        "body", n_links, cast(PhysicalBody, mb.virtualRoot()), htype=HingeType.BALL, params=params
    )

    # finalize and verify multibody
    mb.ensureCurrent()
    mb.resetData()
    assert allFinalized()

    return mb

n_links = 5
mb = createMbody(n_links)

# get the first body
bd1 = mb.getBody("body_0")

# get the second body
bd2 = mb.getBody("body_1")

Setup the kdFlex Scene

Next we setup kdflex’s graphics by calling the setupGraphics helper method on the multibody. This method takes care of setting up the graphics environment.

See Visualization and SceneLayer for more information relating to this section.

cleanup_graphics, web_scene = mb.setupGraphics(port=0, axes=0.5)
# position the viewpoint camera of the visualization:
web_scene.defaultCamera().pointCameraAt([-4, 8, -n_links / 2 + 2], [0, 0, -n_links / 2], [0, 0, 1])
web_scene._setShadows([0, 0, -1])

# get the proxy scene
proxy_scene = mb.getScene()

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

To our SceneLayer, we register a Karana.Scene.CoalScene instance to perform collision and distance queries using the geometries populated in Karana.Scene.ProxyScene. CoalScene is a wrapper of the open source Coal project.

See Collisions for more information.

col_scene = CoalScene("collision_scene")
proxy_scene.registerClientScene(col_scene, mb.virtualRoot(), layers=LAYER_COLLISION)

Setup Geometries

For the pendulum, we create 3d geometries with associated material properties and attach them to bodies. This is commonly useful for collision detection and 3d visualization.

We then add geometries for the floor, wall, and top pendulum connector

# alternate geometries
box_geom = BoxGeometry(0.1, 0.1, 0.1)
capsule_geom = CapsuleGeometry(0.1, 0.15)

# create an obstacle wall  to be placed in the path of the pendulum
obstacle_geom = BoxGeometry(0.5, 1.8, n_links + 0.1)

# create a ground for the pendulum to collide with
ground_geom = BoxGeometry(7.5, 7.5, 0.5)

# create a connection part for the top of the pendulum
connection_geom = BoxGeometry(0.25, 0.25, 0.1)

# create various visual materials to color the bodies
mat_info = PhysicalMaterialInfo()
mat_info.color = Color.BLUE
blue = PhysicalMaterial(mat_info)
mat_info.color = Color.RED
red = PhysicalMaterial(mat_info)
mat_info.color = Color.DARKSEAGREEN
seagreen = PhysicalMaterial(mat_info)
mat_info.color = Color.BEIGE
beige = PhysicalMaterial(mat_info)
mat_info.color = Color.BLACK
black = PhysicalMaterial(mat_info)

# manually add some objects that are not attached to the pendulum (implictly attached to the root frame)
root_scene_node = ProxySceneNode("root_scene_node", scene=proxy_scene)

# add the wall
obstacle_part = ProxyScenePart(
    "obstacle_part", scene=proxy_scene, geometry=obstacle_geom, material=seagreen
)
obstacle_part.setTranslation([2.75, 0, -n_links / 2 - 0.1])

# add the ground
ground = ProxyScenePart(
    "ground", scene=proxy_scene, geometry=ground_geom, material=beige, layers=LAYER_ALL
)
ground.setTranslation([0, 0, -n_links - 0.25])

# add an upper connection point for the pendulum that is purely visual
connection = ProxyScenePart(
    "ground", scene=proxy_scene, geometry=connection_geom, material=black, layers=LAYER_GRAPHICS
)

del (
    mat_info,
    seagreen,
    black,
    beige,
    connection_geom,
    ground_geom,
    capsule_geom,
    box_geom,
    obstacle_geom,
)

Because we are procedurally generating our pendulum, this dictionary is initialized to store each body and its scenePart. We also add axes for visualization

# creating an empty dictionary which is populated
# by scene parts from the multibody
scene_part_dict = {}
# visualize axes for important frames
root_scene_node.graphics().showAxes(0.5)

# loop through each body to add sceneparts to a dictionary
for link in list(range(0, n_links)):
    body_string = f"body_{link}"
    part_string = f"body_{link}_part"

    # retrieve the sphere scenepart visual geometries to work with later. We only want the spheres so index for 0
    scene_part_dict[part_string] = mb.getBody(body_string).getSceneParts()[0]

    # add axes to object
    scene_part_dict[part_string].graphics().showAxes(0.5)

Setup State Propagator and Models

Now, we setup the Karana.Dynamics.StatePropagator and register our models as well.

See Models for more concepts and information.

# set up state propagator
sp = StatePropagator(mb, IntegratorType.CVODE)
integ = sp.getIntegrator()

# Create a UniformGravity model, and set it's gravitational acceleration.
ug = UniformGravity("grav_model", sp, mb)
ug.params.g = np.array([0, 0, -9.81])
del ug

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

# sync the simulation time with real-time.
SyncRealTime("sync_real_time", sp, 1.0)

Because our CoalScene managed distance queries and collision detection, we now attach a Karana.Models.PenaltyContact using the CoalScene and ProxyScene to apply counter forces whenever registered bodies collide.

# Set contact force model parameters
hc = HuntCrossley("hunt_crossley_contact")
hc.params.kp = 100000
hc.params.kc = 20000
hc.params.mu = 0.3
hc.params.n = 1.5
hc.params.linear_region_tol = 1e-3

PenaltyContact("penalty_contact", sp, mb, [FrameCollider(proxy_scene, col_scene)], hc)

The following section is responsible for indicating collisions of the pendulum by coloring the scene parts when a collision occur. We utilize Karana.Scene.CoalScene.cachedCollisions() which stores a list of collisions which occured during the last collision scene sweep function, which is ran by the collision model. it loops through each Karana.Scene.CollisionInfo object and uses the information to lookup the scene parts corresponding the colliding body and then recolors it. The col_mat_reset_list is a way to recolor the last collisions after the collision has stopped occuring.

col_mat_reset_list = []


def post_step_fn(t, x):
    """Change material color based on collision status.

    If bodies are is colliding, make their materials red. Otherwise, make them blue.

    Parameters
    ----------
    t : float
        The current time.
    x : NDArray[np.float64]
       The current state.
    """
    # reset the materials of the last collision scene parts
    global col_mat_reset_list
    if col_mat_reset_list:
        for mat_reset_part in col_mat_reset_list:
            mat_reset_part.setMaterial(blue)
        col_mat_reset_list = []

    # check if there are any cached collisions
    # and if so, loop through them
    # and recolor the scene parts that were involved in the collision
    if col_scene.cachedCollisions():
        for collision in col_scene.cachedCollisions():

            collision_body_1_id = collision.part1
            collision_body_2_id = collision.part2

            col_part_1 = col_scene.lookupPart(collision_body_1_id)
            col_part_2 = col_scene.lookupPart(collision_body_2_id)

            if col_part_1.name() == "collision_scene_sp_collision":
                collision_part = proxy_scene.lookupProxyFromImpl(col_part_1)
                collision_part.setMaterial(red)
                col_mat_reset_list.append(collision_part)

            if col_part_2.name() == "collision_scene_sp_collision":
                collision_part = proxy_scene.lookupProxyFromImpl(col_part_2)
                collision_part.setMaterial(red)
                col_mat_reset_list.append(collision_part)


sp.fns.post_hop_fns["update_and_info"] = post_step_fn

Set Initial State

Before we begin our simulation, we set the initial state for our multibody and statepropagator.

When accessing or modifying generalized coordinates for a subhinge, it is recommended to directly set the subhinge’s values rather than for the entire multibody in order to avoid ambiguity.

# modify the initial multibody state
bd1 = mb.getBody("body_0")
bd1.parentHinge().subhinge(0).setQ([0.0, pi / 3, 0])  # try adding rotation around x or z axis
bd1.parentHinge().subhinge(0).setU([0.0, -0.5, 0.0])  # for a more chaotic pendulum swing

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

# syncs up the graphics
proxy_scene.update()

Register a Timed Event

Because the penalty contact model applies forces proportional to the overlap between two objects in collision, it is necessary to shorten the period of the timed event. Otherwise, if a collision is only detected when 2 objects have significant overlap, unrealistic forces may be applied.

h = np.timedelta64(int(1e7), "ns")
t = TimedEvent("hop_size", h, lambda _: None, False)
t.period = h

# register after time has been initialized
sp.registerTimedEvent(t)
del t

Run the Simulation

Karana.Math.Integrator.advanceTo() can be increased to lengthen the simulation

# print the inistial state
print(f"t = {float(integ.getTime())/1e9}s; x = {integ.getX()}")

sp.advanceTo(5.0)

t = 0.0s; x = [ 0.          1.04719755  0.          0.          0.01721734  0.
  0.         -0.22815648  0.          0.          0.0697692   0.
  0.         -0.03650634  0.          0.         -0.5         0.
  0.         -0.21945927  0.          0.          0.97027462  0.
  0.          2.59715372  0.          0.         -2.51238425  0.        ]

<SpStatusEnum.REACHED_END_TIME: 1>

Clean Up the Simulation

Below, we cleanup our simulation. We first delete local variables, cleanup our visualizer, discard remaining Karana objects, and optionally verify using allDestroyed.

def cleanup():
    """Cleanup the simulation."""
    global bc, col_scene, web_scene, proxy_scene
    global integ, bd1, bd2, ground, obstacle_part
    global root_scene_node, connection, red, blue, col_mat_reset_list, scene_part_dict
    del (
        integ,
        bd1,
        bd2,
        proxy_scene,
        web_scene,
        col_scene,
        ground,
        obstacle_part,
        root_scene_node,
        connection,
        red,
        blue,
        col_mat_reset_list,
        scene_part_dict,
    )

    discard(sp)
    cleanup_graphics()

    fc = mb.frameContainer()
    discard(mb)
    discard(fc)

    bc = BaseContainer.singleton()
    bc.at_exit_fns.executeReverse()
    bc.at_exit_fns.clear()
    del bc


atexit.register(cleanup)

<function __main__.cleanup()>

Summary

You now know how to setup a n-link pendulum with obstacles to simulate collisions. By attaching a CoalScene and creating a PenaltyContact model, the simulation is able to model collisions. And using a dictionary to track SceneParts for each body allowed us to visualize collisions by making the material red.

Further Readings

Drive an ATRVjr rover