ATRVjr Driving#
In this notebook we import the ATRVjr rover model into kdflex and drive it around using a simple manual applied torque approach. Contact between the wheels and the ground are currently handled using CoalScene which is a wrapper of the open source Coal project.
Requirements:
In this tutorial we will:
Import and modify the URDF
Setup the kdFlex scene
Attach wheel geometries
Setup StatePropagator and models
Setup the Simulation
Run and clean up
Acknowledgements
The robot URDF model included in this project is Copyright (c) 2018 diasdm and licensed under the BSD 2‑Clause License (link). Some modifications were made to adapt the model for use within the kdflex simulation framework, including simplifying wheel geometries due to current rendering and collision handling limitations.
For a more in-depth descriptions of kdflex concepts see usage.
import atexit
import numpy as np
from math import pi
from pathlib import Path
from Karana.Core import discard
from Karana.Frame import FrameContainer
from Karana.Dynamics.SOADyn_types import MultibodyDS
from Karana.Math import IntegratorType
from Karana.Dynamics import (
Multibody,
PhysicalBody,
PhysicalBody,
HingeType,
PhysicalHinge,
PinSubhinge,
StatePropagator,
TimedEvent,
)
from Karana.Dynamics.SOADyn_types import (
HingeDS,
PinSubhingeDS,
SubhingeDS,
Linear3SubhingeDS,
SphericalSubhingeDS,
BodyWithContextDS,
ScenePartSpecDS,
)
from Karana.Scene import (
AssimpImporter,
SphereGeometry,
BoxGeometry,
CylinderGeometry,
StaticMeshGeometry,
AbstractStaticGeometry,
Color,
PhysicalMaterialInfo,
PhysicalMaterial,
LAYER_COLLISION,
LAYER_GRAPHICS,
LAYER_ALL,
)
from Karana.Scene import CoalScene
from Karana.Scene import ProxyScene, ProxySceneNode, ProxyScenePart
from Karana.Math import UnitQuaternion, SpatialInertia, HomTran
from Karana.Models import UniformGravity, UpdateProxyScene, SyncRealTime, PenaltyContact
from Karana.Collision import FrameCollider, HuntCrossley
Build the filepath to the URDF file we will import
resources = Path().absolute().parent / "resources"
urdf_file = resources / "atrvjr" / "atrvjr.urdf"
Import and Modify the URDF#
Import the URDF file to create a SOADyn_types.MultibodyDS. The “DS” is short for “Data Structure”. This is kdFlex’s native, file-format agnostic way of specifying a multibody. We’ll use this later on to instantiate the multibody.
It’s easier to edit the model while it’s in the MultibodyDS format before we convert it to a Multibody. In the cell below we the original hinge between the rover and the root frame to a full 6-DOF hinge, which allows it to move freely. We then convert the hinges between the wheel bodies to pin joints so the wheels can properly rotate.
Last we set all the visuals in the MultibodyDS to belong to the graphics layer only so that it can’t interact with the wheels which we add later or the ground.
mb_info = MultibodyDS.fromUrdf(urdf_file)
mb_info.model_dump()
# convert to full6DOF hinge bewteen root frame and rover.
mb_info.base_bodies[0].body.hinge = HingeDS(
hinge_type=HingeType.FULL6DOF,
subhinges=[
Linear3SubhingeDS(prescribed=False),
SphericalSubhingeDS(prescribed=False),
],
)
# unlock locked Hinges for wheels from the URDF
for i in [0, 1]:
mb_info.base_bodies[0].children[i].body.hinge = HingeDS(
hinge_type=HingeType.PIN,
subhinges=[PinSubhingeDS(prescribed=False, unit_axis=np.array([0.0, 0.0, 1.0]))],
)
for i in [2, 3]:
mb_info.base_bodies[0].children[i].body.hinge = HingeDS(
hinge_type=HingeType.PIN,
subhinges=[PinSubhingeDS(prescribed=False, unit_axis=np.array([0.0, 0.0, -1.0]))],
)
# Remove all collision meshes. We want to add these manually.
def removeCollision():
"""Remove all the collision meshes."""
def _removeParts(parts: list[ScenePartSpecDS]):
idx = []
for k, p in enumerate(parts):
if p.layers == LAYER_COLLISION:
idx.append(k)
idx.reverse()
for k in idx:
parts.pop(k)
def _recurse(b: BodyWithContextDS):
_removeParts(b.body.scene_parts)
for c in b.children:
_recurse(c)
for base in mb_info.base_bodies:
_recurse(base)
removeCollision()
Create a Karana.Frame.FrameContainer to manager the frames layer.
fc = FrameContainer("root")
Instantiate and configure the Karana.Dynamics.Multibody based on the MultibodyDS description.
mb = mb_info.toMultibody(fc)
Finalize the model to get it ready for use.
mb.ensureCurrent()
mb.resetData()
mb.dumpTree()
|-atrvjr_MBVROOT_
|-center_link
|-left_front_wheel_link
|-left_rear_wheel_link
|-right_front_wheel_link
|-right_rear_wheel_link
|-cbimu_frame
|-mpuimu_frame
|-arm_base_frame
|-lrf_frame
|-novatel
Setup the kdFlex Scene#
Next we setup kdflex’s visualizer by creating a graphics scene and a webserver to view it. We use the helper method setupGraphics() to take care of setting up the scene and webserver for us.
See Visualization and SceneLayer for more information relating to this section.
cleanup_graphics, web_scene = mb.setupGraphics(port=0)
web_scene.defaultCamera().pointCameraAt(
offset=np.array([[0.0], [5.0], [10.0]]),
target=np.array([[0.0], [0.0], [0.0]]),
up=np.array([[0.0], [0.0], [1.0]]),
)
proxy_scene = mb.getScene()
[WebUI] Listening at http://newton:33859
# attach a collision scene
col_scene = CoalScene("collision_scene")
proxy_scene.registerClientScene(col_scene, mb.virtualRoot(), layers=LAYER_COLLISION)
Display the bodies in the multibody model.
mb.displayModel()
Body Parent Hinge Dofs
____ ______ _____ ____
1. center_link atrvjr_MBVROOT_ FULL6DOF 6
2. left_front_wheel_link center_link PIN 1
3. left_rear_wheel_link center_link PIN 1
4. right_front_wheel_link center_link PIN 1
5. right_rear_wheel_link center_link PIN 1
6. cbimu_frame center_link LOCKED 0
7. mpuimu_frame center_link LOCKED 0
8. arm_base_frame center_link LOCKED 0
9. lrf_frame center_link LOCKED 0
10. novatel center_link LOCKED 0
____
10
Nodes
---
Attach Wheel Geometries#
wheel_geom = CylinderGeometry(0.125, 0.075)
# create a ground for the rover to drive on
ground_geom = BoxGeometry(30, 30, 0.25)
# # Importing the actual wheel mesh. Using sphere's instead though since these are wonky for collision dynamics
# wheel_file = "/home/griffith/workspace/MySandbox/src/Tutorials/test/resources/atrvjr/meshes/wheel.obj"
# importer = AssimpImporter()
# wheel_obj = importer.importFrom(wheel_file)
left_front_wheel = mb.getBody("left_front_wheel_link")
# left_front_wheel.addScenePartSpec(wheel_obj.parts[0])
# left_front_wheel.addScenePartSpec(wheel_obj.parts[1])
left_front_wheel_node = ProxyScenePart(
name="left_front_wheel_link_node", scene=proxy_scene, geometry=wheel_geom
)
left_front_wheel_node.attachTo(left_front_wheel)
left_front_wheel_node.setTranslation([0, 0, 0.03])
left_front_wheel_node.setUnitQuaternion(UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))
# left_front_wheel_node.graphics().showAxes(1.0)
left_rear_wheel = mb.getBody("left_rear_wheel_link")
# left_rear_wheel.addScenePartSpec(wheel_obj.parts[0])
# left_rear_wheel.addScenePartSpec(wheel_obj.parts[1])
left_rear_wheel_node = ProxyScenePart(
"left_rear_wheel_link_node", scene=proxy_scene, geometry=wheel_geom
)
left_rear_wheel_node.attachTo(left_rear_wheel)
left_rear_wheel_node.setTranslation([0, 0, 0.03])
left_rear_wheel_node.setUnitQuaternion(UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))
# left_rear_wheel_node.graphics().showAxes(1.0)
right_front_wheel = mb.getBody("right_front_wheel_link")
# right_front_wheel.addScenePartSpec(wheel_obj.parts[0])
# right_front_wheel.addScenePartSpec(wheel_obj.parts[1])
right_front_wheel_node = ProxyScenePart(
"right_front_wheel_link_node", scene=proxy_scene, geometry=wheel_geom
)
right_front_wheel_node.attachTo(right_front_wheel)
right_front_wheel_node.setTranslation([0, 0, 0.03])
right_front_wheel_node.setUnitQuaternion(UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))
# right_front_wheel_node.graphics().showAxes(1.0)
right_rear_wheel = mb.getBody("right_rear_wheel_link")
# right_rear_wheel.addScenePartSpec(wheel_obj.parts[0])
# right_rear_wheel.addScenePartSpec(wheel_obj.parts[1])
right_rear_wheel_node = ProxyScenePart(
"right_rear_wheel_link_node", scene=proxy_scene, geometry=wheel_geom
)
right_rear_wheel_node.attachTo(right_rear_wheel)
right_rear_wheel_node.setTranslation([0, 0, 0.03])
right_rear_wheel_node.setUnitQuaternion(UnitQuaternion(pi / 2, [1.0, 0.0, 0.0]))
# right_rear_wheel_node.graphics().showAxes(1.0)
mat_info = PhysicalMaterialInfo()
mat_info.color = Color.WHITESMOKE
gray = PhysicalMaterial(mat_info)
# # exporter currently struggles to export the full multibody
# multibodyDS = mb.toDS()
# multibodyDS.toFile("atrvjr.json")
ground = ProxyScenePart(
"ground", scene=proxy_scene, geometry=ground_geom, material=gray, layers=LAYER_ALL
)
ground.setTranslation([0, 0, -0.1])
del ground, mat_info, wheel_geom, ground_geom, gray
Setup StatePropagator and models#
# set up state propagator
sp = StatePropagator(mb, IntegratorType.RK4)
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)
# add penalty contact model and register its bodies and 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)
del col_scene, hc
Because forces are cleared out in between simulation steps, we create a callback function to apply forces depending on the intended direction. We attach it as a Karana.Dynamics.spFunctions.pre_deriv_fns, since it is called before the multibody derivative which ensures the forces are used in the state update calculations.
direction = "coast"
def pre_deriv_fn(t, x):
"""Print out the time and state.
Parameters
----------
t : float
The current time.
x : NDArray[np.float64]
The current state.
"""
# manually set torques in pre_deriv function
global direction
match direction:
case "coast":
right_rear_wheel.parentHinge().subhinge(0).setT([0])
left_rear_wheel.parentHinge().subhinge(0).setT([0])
right_front_wheel.parentHinge().subhinge(0).setT([0])
left_front_wheel.parentHinge().subhinge(0).setT([0])
case "forward":
right_rear_wheel.parentHinge().subhinge(0).setT([4])
left_rear_wheel.parentHinge().subhinge(0).setT([4])
right_front_wheel.parentHinge().subhinge(0).setT([4])
left_front_wheel.parentHinge().subhinge(0).setT([4])
case "backward":
right_rear_wheel.parentHinge().subhinge(0).setT([-4])
left_rear_wheel.parentHinge().subhinge(0).setT([-4])
right_front_wheel.parentHinge().subhinge(0).setT([-4])
left_front_wheel.parentHinge().subhinge(0).setT([-4])
case "rotate_right":
right_rear_wheel.parentHinge().subhinge(0).setT([-5])
left_rear_wheel.parentHinge().subhinge(0).setT([5])
right_front_wheel.parentHinge().subhinge(0).setT([-5])
left_front_wheel.parentHinge().subhinge(0).setT([5])
case "rotate_left":
right_rear_wheel.parentHinge().subhinge(0).setT([5])
left_rear_wheel.parentHinge().subhinge(0).setT([-5])
right_front_wheel.parentHinge().subhinge(0).setT([5])
left_front_wheel.parentHinge().subhinge(0).setT([-5])
sp.fns.pre_deriv_fns["apply_torque"] = pre_deriv_fn
Setup the Simulation#
Finish by setting the initial states and timed events
# 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 timed event
h = np.timedelta64(int(1e7), "ns")
t = TimedEvent("hop_size", h, lambda _: None, False)
t.period = h
sp.registerTimedEvent(t)
del t
Run and Clean Up#
direction = "coast"
sp.advanceBy(5.0)
direction = "forward"
sp.advanceBy(3.0)
direction = "backward"
sp.advanceBy(6.0)
direction = "forward"
sp.advanceBy(3.0)
direction = "rotate_right"
sp.advanceBy(3.0)
direction = "rotate_left"
sp.advanceBy(1.5)
direction = "forward"
sp.advanceBy(4.0)
direction = "backward"
sp.advanceBy(4.0)
direction = "rotate_right"
sp.advanceBy(3.0)
direction = "rotate_left"
sp.advanceBy(1.5)
direction = "forward"
sp.advanceBy(4.0)
direction = "backward"
sp.advanceBy(4.0)
direction = "rotate_right"
sp.advanceBy(3.0)
direction = "rotate_left"
sp.advanceBy(1.5)
direction = "forward"
sp.advanceBy(4.0)
<SpStatusEnum.REACHED_END_TIME: 1>
# Cleanup
def cleanup():
"""Cleanup the simulation."""
global left_front_wheel, left_front_wheel_node, left_rear_wheel, left_rear_wheel_node, right_rear_wheel, right_rear_wheel_node, right_front_wheel, right_front_wheel_node, proxy_scene, mb_info, integ, web_scene
del (
left_front_wheel,
left_front_wheel_node,
left_rear_wheel,
left_rear_wheel_node,
right_rear_wheel,
right_rear_wheel_node,
right_front_wheel,
right_front_wheel_node,
proxy_scene,
web_scene,
mb_info,
integ,
)
discard(sp)
cleanup_graphics()
discard(mb)
discard(fc)
atexit.register(cleanup)
<function __main__.cleanup()>
Summary#
You now understand how to drive a ATRVjr model by applying torque forces in the PreDeriv call.