# Copyright (c) 2024-2025 Karana Dynamics Pty Ltd. All rights reserved.
#
# NOTICE TO USER:
#
# This source code and/or documentation (the "Licensed Materials") is
# the confidential and proprietary information of Karana Dynamics Inc.
# Use of these Licensed Materials is governed by the terms and conditions
# of a separate software license agreement between Karana Dynamics and the
# Licensee ("License Agreement"). Unless expressly permitted under that
# agreement, any reproduction, modification, distribution, or disclosure
# of the Licensed Materials, in whole or in part, to any third party
# without the prior written consent of Karana Dynamics is strictly prohibited.
#
# THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
# KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
# BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
# FITNESS FOR A PARTICULAR PURPOSE.
#
# IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
# INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
# OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
#
# U.S. Government End Users: The Licensed Materials are a "commercial item"
# as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
# only as a commercial end item under the terms of this license.
#
# Any use of the Licensed Materials in individual or commercial software must
# include, in the user documentation and internal source code comments,
# this Notice, Disclaimer, and U.S. Government Use Provision.
import warnings
import re
from collections.abc import Callable
from typing import Protocol, runtime_checkable
import numpy as np
from Karana.KUtils.Kquantities import convert as convertUnits
from Karana.Math import HomTran, SpatialVector, UnitQuaternion
from Karana.Dynamics import Multibody, StatePropagator, Node
from Karana.Models import KModel
from Karana.Scene import (
ProxyScene,
ProxyScenePart,
CylinderGeometry,
ConeGeometry,
ProxySceneNode,
Color,
WebScene,
LAYER_ORNAMENTAL,
PhongMaterialInfo,
PhongMaterial,
)
from Karana.Collision import ContactForceBase
__all__ = [
"NodeForceVisualizer",
"LastContactVisualizer",
"SpringForceVisualizer",
"SpringModelProtocol",
]
[docs]
class NodeForceVisualizer:
"""This class is a helper to visualize the force on a Node.
The force is visualized as an arrow emanating from the point where
the force is applied. The arrow's length scales based on the
magnitude of the force.
Unless constructed with auto_update=True, the update
method must be called periodically to update the arrow.
"""
def __init__(
self,
name: str,
scene: ProxyScene,
force_scale: float | Callable = 0.01,
radius: float = 0.01,
color: Color = Color.WHITE,
auto_update: bool = True,
):
"""Create an NodeForceVisualizer instance.
Parameters
----------
name: str
Name of the NodeForceVisualizer instance
scene: ProxyScene
The scene manager
radius: float
Radius of the arrow. Defaults to 0.01.
force_scale: float | Callable
Used to scale the length of the force array based on the
applied force. If a float, it is multiplied by the force
to get the length. If a callable, it is called with the
force magnitude as an argument to get the length.
Defaults to 0.01.
color: Color
The color of the arrow. Defaults to white.
auto_update: bool
If true (the default), the scene automatically updates this
"""
mat_info = PhongMaterialInfo()
mat_info.color = Color.BLACK
mat_info.ambient_color = Color.BLACK
mat_info.emissive_color = color
mat = PhongMaterial(mat_info)
self._scene = scene
if isinstance(force_scale, (int, float)):
self._scale_func = lambda x: x * force_scale
else:
self._scale_func = force_scale
self._node = None
self._scene_node = ProxySceneNode(f"{name}_nd", scene)
self._scene_node.setVisible(False)
self._rod = ProxyScenePart(
f"{name}_rod",
scene=scene,
geometry=CylinderGeometry(radius=0.5 * radius, height=0.75),
material=mat,
layers=LAYER_ORNAMENTAL,
)
self._rod.attachTo(self._scene_node)
self._rod.setTranslation([0, 0.75 / 2, 0])
self._tip = ProxyScenePart(
f"{name}_tip",
scene=scene,
geometry=ConeGeometry(radius=radius, height=0.25),
material=mat,
layers=LAYER_ORNAMENTAL,
)
self._tip.attachTo(self._scene_node)
self._tip.setTranslation([0, 0.75 + 0.25 / 2, 0])
self._scene.update_callbacks[f"{name}_node_force_viz_update"] = self.update
[docs]
def setNode(self, node: Node | None, update: bool = True):
"""Set the external force node to visualize.
Parameters
----------
node: Node | None
The node to visualize the force of, or None to disable
update: bool
If True, immediately update the force visualization
"""
if node and node.parentBody().isRootBody():
# Treat virtual root as no node
node = None
# Skip doing a bunch of unnecessary work if it's the same node
if self._node == node:
return
self._node = node
if self._node:
self._scene_node.attachTo(self._node)
else:
self._scene_node.detach()
self._scene_node.setVisible(False)
if update:
self.update()
else:
self._scene_node.setVisible(False)
[docs]
def update(self):
"""Update the force visualization.
Unless constructed with auto_update=True, this needs
to be called whenever the force changes in order to
see the effect in the visualization.
"""
if not self._node:
return
fext = self._node.getSpForce().getv()
magnitude = np.linalg.norm(fext)
if magnitude < 1e-13:
# Bail out to avoid a potential singularity
self._scene_node.setVisible(False)
return
length = self._scale_func(magnitude)
self._scene_node.setScale(length)
self._rod.setIntrinsicScale([1 / length, 1, 1 / length])
self._tip.setIntrinsicScale([1 / length, 1, 1 / length])
quat = UnitQuaternion([0, 1, 0], fext)
self._scene_node.setUnitQuaternion(quat)
self._scene_node.setVisible(True)
[docs]
@runtime_checkable
class SpringModelProtocol(Protocol):
"""A protocol for classes representing a spring model between a pair of nodes."""
[docs]
def sourceNode(self) -> Node:
"""Return the starting node of the spring."""
...
[docs]
def targetNode(self) -> Node:
"""Return the ending node of the spring."""
...
[docs]
class SpringForceVisualizer:
"""This class is a helper to visualize forces generated by contact.
The force is visualized as a pair of arrows emanating from the
point where the force is applied. The arrow's length scales based
on the magnitude of the force.
Unless constructed with auto_update=True, the update
method must be called periodically to update the arrows.
"""
def __init__(
self,
name: str,
mdl: SpringModelProtocol,
scene: ProxyScene,
*,
colors: tuple[Color, Color] = (Color.YELLOW, Color.BLUE),
force_scale: float | Callable = 0.01,
radius: float = 0.01,
auto_update: bool = True,
):
"""Create a SpringForceVisualizer instance.
Parameters
----------
name: str
Name of the LastContactVisualizer instance
mdl: SpringModelProtocol
The spring force model
scene: ProxySCene
The scene manager
colors: tuple[Color, Color]
Colors of the source and target node arrows.
Defaults to yellow and blue, respectively.
radius: float
Radius of the arrows. Defaults to 0.01.
force_scale: float | Callable
Used to scale the length of the force array based on the
applied force. If a float, it is multiplied by the force
to get the length. If a callable, it is called with the
force magnitude as an argument to get the length.
Defaults to 0.01.
auto_update: bool
If true (the default), the scene automatically updates this
"""
if not isinstance(mdl, SpringModelProtocol):
raise TypeError(
f"Model {mdl.name()} doesn't have a valid sourceNode and/or targetNode getter"
)
self._model = mdl
self._force_viz1 = NodeForceVisualizer(
name=f"{name}1",
scene=scene,
color=colors[0],
radius=radius,
force_scale=force_scale,
auto_update=auto_update,
)
self._force_viz2 = NodeForceVisualizer(
name=f"{name}2",
scene=scene,
color=colors[1],
radius=radius,
force_scale=force_scale,
auto_update=auto_update,
)
self._force_viz1.setNode(mdl.sourceNode(), update=True)
self._force_viz2.setNode(mdl.targetNode(), update=True)
[docs]
def update(self, *args):
"""Update the force visualization.
Unless constructed with auto_update=True, this needs to be called
whenever to the force changes in order to see the effect in the
visualization.
"""
self._force_viz1.update()
self._force_viz2.update()
