{
"cells": [
{
"cell_type": "markdown",
"id": "93688312",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"# Procedural n-link pendulum collision example\n",
"\n",
"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.\n",
"\n",
"Requirements:\n",
"- [n-link Pendulum](../example_n_link_pendulum/notebook.ipynb)\n",
"- [2-link Pendulum Collision](../example_pendulum_collision/notebook.ipynb)\n",
"\n",
"\n",
"In this example we will:\n",
" - Create the n-link pendulum multibody\n",
" - Create the **kdFlex** Scene\n",
" - Setup geometries\n",
" - Set up the state propagator and models\n",
" - Set initial state\n",
" - Register a timed event\n",
" - Run the simulation\n",
" - Clean up the simulation\n",
"\n",
"\n",
"\n",
"For a more in-depth descriptions of **kdflex** concepts see [usage](usage_page)."
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "ce9260c1",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"import numpy as np\n",
"from typing import cast\n",
"from math import pi\n",
"import atexit\n",
"\n",
"import Karana.Core as kc\n",
"import Karana.Math as km\n",
"import Karana.Integrators as ki\n",
"import Karana.Frame as kf\n",
"import Karana.Dynamics as kd\n",
"import Karana.Dynamics.SOADyn_types as kdt\n",
"import Karana.Scene as ks\n",
"import Karana.KUtils as ku\n",
"import Karana.Models as kmdl\n",
"import Karana.Collision as kcoll\n",
"from Karana.KUtils.Sim import Sim"
]
},
{
"cell_type": "markdown",
"id": "cf3ac55b",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Create the n-link Pendulum Multibody\n",
"\n",
"See [Multibody](treembody_sec) or [Frames](frames_layer_sec) for more information relating to this step."
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "c24f60d7",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"def populateMbody(n_links: int, sim: Sim) -> None:\n",
" \"\"\"Populate the Multibody.\n",
"\n",
" Parameters\n",
" ----------\n",
" n_links : int\n",
" Number of links in the pendulum system.\n",
" sim : Sim\n",
" The Simulation whose Multibody will be populated.\n",
" \"\"\"\n",
" mb = sim.mb\n",
"\n",
" # inertia used for both bodies\n",
" spI = km.SpatialInertia(2.0, np.zeros(3), np.diag([3, 2, 1]))\n",
"\n",
" # inboard to joint transform used for both bodies\n",
" ib2j = km.HomTran()\n",
"\n",
" # body to joint transform used for both bodies\n",
" b2j = km.HomTran(np.array([0, 0, 1.0]))\n",
"\n",
" # adding colored material\n",
" mat_info = ks.PhysicalMaterialInfo()\n",
" mat_info.color = ks.Color.BLUE\n",
" blue = ks.PhysicalMaterial(mat_info)\n",
" mat_info.color = ks.Color.LIGHTBLUE\n",
" light_blue = ks.PhysicalMaterial(mat_info)\n",
"\n",
" # Add a scene part that is for collisions and\n",
" sp_col = ks.ScenePartSpec()\n",
" sp_col.name = \"sp_collision\"\n",
" sp_col.geometry = ks.SphereGeometry(0.3)\n",
" sp_col.material = blue\n",
" sp_col.transform = km.HomTran([0.0, 0.0, 0.0])\n",
" sp_col.scale = [1, 1, 1]\n",
" # LAYER_PHYSICAL is default ScenePart layer which this part belongs to\n",
"\n",
" # Add a scene part that is purely visual\n",
" sp_vis = ks.ScenePartSpec()\n",
" sp_vis.name = \"sp_graphics\"\n",
" sp_vis.geometry = ks.CylinderGeometry(0.025, 1.0)\n",
" sp_vis.material = light_blue\n",
" sp_vis.transform = km.HomTran(q=km.UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]), vec=[0.0, 0.0, 0.5])\n",
" sp_vis.scale = [1, 1, 1]\n",
" sp_vis.layers = ks.LAYER_PHYSICAL_GRAPHICS\n",
"\n",
" # params for each body in pendulum\n",
" params = kd.PhysicalBodyParams(\n",
" spI=spI,\n",
" axes=[np.array([0.0, 1.0, 0.0])],\n",
" body_to_joint_transform=b2j,\n",
" inb_to_joint_transform=ib2j,\n",
" scene_part_specs=[sp_col, sp_vis],\n",
" )\n",
"\n",
" kd.PhysicalBody.addSerialChain(\n",
" \"body\",\n",
" n_links,\n",
" cast(kd.PhysicalBody, mb.virtualRoot()),\n",
" htype=kd.HingeType.BALL,\n",
" params=params,\n",
" )\n",
"\n",
" # finalize and verify multibody\n",
" mb.ensureHealthy()\n",
" mb.resetData()\n",
" assert kc.allReady()"
]
},
{
"cell_type": "code",
"execution_count": 3,
"id": "faf0393e",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"n_links = 5\n",
"sim = Sim()\n",
"populateMbody(n_links, sim)\n",
"\n",
"# get the first body\n",
"bd1 = sim.mb.getBody(\"body_0\")\n",
"\n",
"# get the second body\n",
"bd2 = sim.mb.getBody(\"body_1\")"
]
},
{
"cell_type": "markdown",
"id": "76626dc6",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Setup the kdFlex Scene\n",
"\n",
"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. \n",
"\n",
"See [Visualization](visualization_sec) and [Scene Layer](scene_layer_sec) for more information relating to this section."
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "c5c88b33",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"data": {
"text/html": [
"\n",
" \n",
" \n",
"
\n",
" "
],
"text/plain": [
""
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"_, web_scene = sim.setupGraphics(port=0, axes=0.5)\n",
"# position the viewpoint camera of the visualization:\n",
"web_scene.defaultCamera().pointCameraAt([-4, 8, -n_links / 2 + 2], [0, 0, -n_links / 2], [0, 0, 1])\n",
"web_scene._setShadows([0, 0, -1])\n",
"\n",
"# get the proxy scene\n",
"proxy_scene = sim.mb.getScene()"
]
},
{
"cell_type": "markdown",
"id": "759c8aab",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"To our scene layer, we register a {py:class}`Karana.Scene.CoalScene` instance to perform collision and distance queries using the geometries populated in {py:class}`Karana.Scene.ProxyScene`. CoalScene is a wrapper of the open source [Coal](https://github.com/coal-library/coal) project.\n",
"\n",
"See [Collisions](collision_dynamics_sec) for more information."
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "62d7c311",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"col_scene = ks.CoalScene(\"collision_scene\")\n",
"proxy_scene.registerClientScene(col_scene, sim.mb.virtualRoot(), layers=ks.LAYER_COLLISION)"
]
},
{
"cell_type": "markdown",
"id": "47dcf2d1",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Setup Geometries\n",
"\n",
"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.\n",
"\n",
"We then add geometries for the floor, wall, and top pendulum connector"
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "debf4014",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# alternate geometries\n",
"box_geom = ks.BoxGeometry(0.1, 0.1, 0.1)\n",
"capsule_geom = ks.CapsuleGeometry(0.1, 0.15)\n",
"\n",
"# create an obstacle wall to be placed in the path of the pendulum\n",
"obstacle_geom = ks.BoxGeometry(0.5, 1.8, n_links + 0.1)\n",
"\n",
"# create a ground for the pendulum to collide with\n",
"ground_geom = ks.BoxGeometry(7.5, 7.5, 0.5)\n",
"\n",
"# create a connection part for the top of the pendulum\n",
"connection_geom = ks.BoxGeometry(0.25, 0.25, 0.1)\n",
"\n",
"# create various visual materials to color the bodies\n",
"mat_info = ks.PhysicalMaterialInfo()\n",
"mat_info.color = ks.Color.BLUE\n",
"blue = ks.PhysicalMaterial(mat_info)\n",
"mat_info.color = ks.Color.RED\n",
"red = ks.PhysicalMaterial(mat_info)\n",
"mat_info.color = ks.Color.DARKSEAGREEN\n",
"seagreen = ks.PhysicalMaterial(mat_info)\n",
"mat_info.color = ks.Color.BEIGE\n",
"beige = ks.PhysicalMaterial(mat_info)\n",
"mat_info.color = ks.Color.BLACK\n",
"black = ks.PhysicalMaterial(mat_info)\n",
"\n",
"# manually add some objects that are not attached to the pendulum (implicitly attached to the root frame)\n",
"root_scene_node = ks.ProxySceneNode(\"root_scene_node\", scene=proxy_scene)\n",
"\n",
"# add the wall\n",
"obstacle_part = ks.ProxyScenePart(\n",
" \"obstacle_part\", scene=proxy_scene, geometry=obstacle_geom, material=seagreen\n",
")\n",
"obstacle_part.setTranslation([2.75, 0, -n_links / 2 - 0.1])\n",
"\n",
"# add the ground\n",
"ground = ks.ProxyScenePart(\n",
" \"ground\", scene=proxy_scene, geometry=ground_geom, material=beige, layers=ks.LAYER_PHYSICAL\n",
")\n",
"ground.setTranslation([0, 0, -n_links - 0.25])\n",
"\n",
"# add an upper connection point for the pendulum that is purely visual\n",
"connection = ks.ProxyScenePart(\n",
" \"ground\",\n",
" scene=proxy_scene,\n",
" geometry=connection_geom,\n",
" material=black,\n",
" layers=ks.LAYER_PHYSICAL_GRAPHICS,\n",
")\n",
"\n",
"del (\n",
" mat_info,\n",
" seagreen,\n",
" black,\n",
" beige,\n",
" connection_geom,\n",
" ground_geom,\n",
" capsule_geom,\n",
" box_geom,\n",
" obstacle_geom,\n",
")"
]
},
{
"cell_type": "markdown",
"id": "95eea2e7",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"Because we are procedurally generating our pendulum, this dictionary is initialized to store each body and its ScenePart. We also add axes for visualization"
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "ef948831",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# creating an empty dictionary which is populated\n",
"# by scene parts from the multibody\n",
"scene_part_dict = {}\n",
"# visualize axes for important frames\n",
"root_scene_node.graphics().showAxes(0.5)\n",
"\n",
"# loop through each body to add SceneParts to a dictionary\n",
"for link in list(range(0, n_links)):\n",
" body_string = f\"body_{link}\"\n",
" part_string = f\"body_{link}_part\"\n",
"\n",
" # retrieve the sphere ScenePart visual geometries to work with later. We only want the spheres so index for 0\n",
" scene_part_dict[part_string] = sim.mb.getBody(body_string).getSceneParts()[0]\n",
"\n",
" # add axes to object\n",
" scene_part_dict[part_string].graphics().showAxes(0.5)"
]
},
{
"cell_type": "markdown",
"id": "771f07f5",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Setup KModels\n",
"\n",
"Now, we setup and register our KModels as well. \n",
"\n",
"See [Models](system_level_models_sec) for more concepts and information."
]
},
{
"cell_type": "code",
"execution_count": 8,
"id": "5a6cf2e2",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"data": {
"text/plain": [
""
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# set up state propagator\n",
"integ = sim.sp.getIntegrator()\n",
"\n",
"# Create a UniformGravity model, and set it's gravitational acceleration.\n",
"ug = kmdl.Gravity(\"grav_model\", sim.sp, kmdl.UniformGravity(\"uniform_gravity\"), sim.mb)\n",
"ug.getGravityInterface().setGravity(np.array([0, 0, -9.81]), 0.0, kmdl.OutputUpdateType.PRE_HOP)\n",
"del ug\n",
"\n",
"# sync the simulation time with real-time.\n",
"kmdl.SyncRealTime(\"sync_real_time\", sim.sp, 1.0)"
]
},
{
"cell_type": "markdown",
"id": "a6890649",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"Because our CoalScene managed distance queries and collision detection, we now attach a {py:class}`Karana.Models.PenaltyContact` using the CoalScene and ProxyScene to apply counter forces whenever registered bodies collide."
]
},
{
"cell_type": "code",
"execution_count": 9,
"id": "8e38c908",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# Set contact force model parameters\n",
"hc = kcoll.HuntCrossley(\"hunt_crossley_contact\")\n",
"hc.params.kp = 100000\n",
"hc.params.kc = 20000\n",
"hc.params.mu = 0.3\n",
"hc.params.n = 1.5\n",
"hc.params.linear_region_tol = 1e-3\n",
"\n",
"kmdl.PenaltyContact(\n",
" \"penalty_contact\", sim.sp, sim.mb, [kcoll.FrameCollider(proxy_scene, col_scene)], hc\n",
")\n",
"del hc"
]
},
{
"cell_type": "markdown",
"id": "62d261df",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"The following section is responsible for indicating collisions of the pendulum\n",
"by coloring the scene parts when a collision occur. We utilize {py:meth}`Karana.Scene.CoalScene.cachedCollisions` which stores a list of collisions which occurred during the\n",
"last collision scene sweep function, which is ran by the collision model.\n",
"it loops through each {py:class}`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 occurring."
]
},
{
"cell_type": "code",
"execution_count": 10,
"id": "4d7c172a",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"col_mat_reset_list = []\n",
"\n",
"\n",
"def post_step_fn(t, x):\n",
" \"\"\"Change material color based on collision status.\n",
"\n",
" If bodies are is colliding, make their materials red. Otherwise, make them blue.\n",
"\n",
" Parameters\n",
" ----------\n",
" t : float\n",
" The current time.\n",
" x : NDArray[np.float64]\n",
" The current state.\n",
" \"\"\"\n",
" # reset the materials of the last collision scene parts\n",
" global col_mat_reset_list\n",
" if col_mat_reset_list:\n",
" for mat_reset_part in col_mat_reset_list:\n",
" mat_reset_part.setMaterial(blue)\n",
" col_mat_reset_list = []\n",
"\n",
" # check if there are any cached collisions\n",
" # and if so, loop through them\n",
" # and recolor the scene parts that were involved in the collision\n",
" if col_scene.cachedCollisions():\n",
" for collision in col_scene.cachedCollisions():\n",
" collision_body_1_id = collision.part1\n",
" collision_body_2_id = collision.part2\n",
"\n",
" col_part_1 = col_scene.lookupPart(collision_body_1_id)\n",
" col_part_2 = col_scene.lookupPart(collision_body_2_id)\n",
"\n",
" if col_part_1.name() == \"collision_scene_sp_collision\":\n",
" collision_part = proxy_scene.lookupProxyFromImpl(col_part_1)\n",
" collision_part.setMaterial(red)\n",
" col_mat_reset_list.append(collision_part)\n",
"\n",
" if col_part_2.name() == \"collision_scene_sp_collision\":\n",
" collision_part = proxy_scene.lookupProxyFromImpl(col_part_2)\n",
" collision_part.setMaterial(red)\n",
" col_mat_reset_list.append(collision_part)\n",
"\n",
"\n",
"sim.sp.fns.post_hop_fns[\"update_and_info\"] = post_step_fn"
]
},
{
"cell_type": "markdown",
"id": "e5de4d87",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Set Initial State\n",
"\n",
"Before we begin our simulation, we set the initial state for our multibody and statepropagator. \n",
"\n",
"When accessing or modifying [generalized coordinates](coords_sec) for a subhinge, it is recommended to directly set the subhinge's values rather than for the entire multibody in order to avoid ambiguity."
]
},
{
"cell_type": "code",
"execution_count": 11,
"id": "3a569ae5",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"# modify the initial multibody state\n",
"bd1 = sim.mb.getBody(\"body_0\")\n",
"bd1.parentHinge().subhinge(0).setQ([0.0, pi / 3, 0]) # try adding rotation around x or z axis\n",
"bd1.parentHinge().subhinge(0).setU([0.0, -0.5, 0.0]) # for a more chaotic pendulum swing\n",
"\n",
"# set integrator state\n",
"sim.sp.hardReset()\n",
"t_init = np.timedelta64(0, \"ns\")\n",
"x_init = sim.sp.assembleState()\n",
"sim.sp.setTime(t_init)\n",
"sim.sp.setState(x_init)\n",
"sim.sp.ensureHealthy()\n",
"\n",
"# syncs up the graphics\n",
"proxy_scene.update()"
]
},
{
"cell_type": "markdown",
"id": "6c71f917",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Register a Timed Event\n",
"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."
]
},
{
"cell_type": "code",
"execution_count": 12,
"id": "88188c58",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [],
"source": [
"h = np.timedelta64(int(1e7), \"ns\")\n",
"t = kd.TimedEvent(\"hop_size\", h, lambda _: None, False)\n",
"t.period = h\n",
"\n",
"# register after time has been initialized\n",
"sim.sp.registerTimedEvent(t)\n",
"del t"
]
},
{
"cell_type": "markdown",
"id": "a5406f59",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Run the Simulation\n",
"\n",
"{py:meth}`Karana.Dynamics.StatePropagator.advanceTo` can be increased to lengthen the simulation"
]
},
{
"cell_type": "code",
"execution_count": 13,
"id": "ca38ba01",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"t = 0.0s; x = [ 0. 1.04719755 0. 0. 0. 0.\n",
" 0. 0. 0. 0. 0. 0.\n",
" 0. 0. 0. 0. -0.5 0.\n",
" 0. 0. 0. 0. 0. 0.\n",
" 0. 0. 0. 0. 0. 0. ]\n"
]
},
{
"data": {
"text/plain": [
""
]
},
"execution_count": 13,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# print the initial state\n",
"print(f\"t = {float(integ.getTime()) / 1e9}s; x = {integ.getX()}\")\n",
"\n",
"sim.sp.advanceTo(5.0)"
]
},
{
"cell_type": "markdown",
"id": "843469f3",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Clean Up the Simulation\n",
"\n",
"Below, we cleanup our simulation. We first delete local variables, cleanup our visualizer, discard remaining Karana objects, and optionally verify using allDestroyed."
]
},
{
"cell_type": "code",
"execution_count": 14,
"id": "75468d8a",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"outputs": [
{
"data": {
"text/plain": [
""
]
},
"execution_count": 14,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"def cleanup():\n",
" \"\"\"Cleanup the simulation.\"\"\"\n",
" global sim, col_scene, web_scene, proxy_scene\n",
" global integ, bd1, bd2, ground, obstacle_part\n",
" global root_scene_node, connection, red, blue, col_mat_reset_list, scene_part_dict\n",
" del (\n",
" integ,\n",
" bd1,\n",
" bd2,\n",
" proxy_scene,\n",
" web_scene,\n",
" col_scene,\n",
" ground,\n",
" obstacle_part,\n",
" root_scene_node,\n",
" connection,\n",
" red,\n",
" blue,\n",
" col_mat_reset_list,\n",
" scene_part_dict,\n",
" sim,\n",
" )\n",
"\n",
" bc = kc.BaseContainer.singleton()\n",
" bc.at_exit_fns.executeAndPopReverse()\n",
" del bc\n",
"\n",
"\n",
"atexit.register(cleanup)"
]
},
{
"cell_type": "markdown",
"id": "9669ec38",
"metadata": {
"jupyter": {
"outputs_hidden": false
}
},
"source": [
"## Summary\n",
"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.\n",
"\n",
"## Further Readings\n",
"[Drive an ATRVjr rover](../example_atrvjr_drive/notebook.ipynb)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3 (ipykernel)",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.12.3"
},
"name": "notebook.ipynb"
},
"nbformat": 4,
"nbformat_minor": 5
}