# Copyright (c) 2024-2026 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.
"""Contains helper classes for creating info card GUIs."""
from abc import abstractmethod, ABC
from typing import (
Callable,
Generic,
SupportsFloat,
SupportsIndex,
TypeVar,
Type,
cast,
get_args,
Any,
)
from Karana.KUtils.DataStruct import IdMixin
from networkx import immediate_dominators
import numpy as np
from numpy.typing import ArrayLike, NDArray
import textwrap
from collections.abc import Iterable, Sequence
import itertools
from pathlib import Path
from dataclasses import dataclass
import traceback
import Karana.Core as kc
import Karana.Math as km
import Karana.WebUI as kw
import Karana.Dynamics as kd
import Karana.Frame as kf
import Karana.Scene as ks
import Karana.Integrators as ki
import Karana.Models as kmdl
from Karana.KUtils import vizutils
import Karana.KUtils.visjs as vjs
import Karana.Collision as kcoll
from Karana.Math.Kquantities import ureg
from ._effects import (
EffectItem,
EffectManager,
AxesParams,
OutlineParams,
GraphEdgeParams,
)
from ._helpers import widgetArray
from ._worker import AsyncWorker
T = TypeVar("T")
class WrappedTypeMixin(Generic[T]):
"""Adds the wrapped method."""
@classmethod
def wrapped(cls) -> Type[T]:
"""Get the concrete type used for T.
Note that this requires a new derived class like so:
>>> class MyWrapper(WrappedTypeMixin[T]):
... pass
...
>>> class IntWrapper(MyWrapper[int]):
... pass
...
>>> IntWrapper.wrapped() == int
True
But this WILL NOT work:
>>> MyWrapper[int].wrapped() == int
False
"""
for base in getattr(cls, "__orig_bases__", []):
if hasattr(base, "__origin__"):
args = get_args(base)
if args:
return args[0]
raise TypeError(f"{cls.__name__} has not been specialized with a type.")
[docs]
@dataclass
class GuiContext:
# the Dock instance
dock: kw.Dock
# The router used to connect widgets
router: kw.Router
# The gui's shared selection state
selection: kw.State
# The multibody for the gui
multibody: kd.Multibody
# The scene manager
scene: ks.ProxyScene
# The scene used for 3d graphics
graphics: ks.WebScene
# Middleware for visual effects
effects: EffectManager
# tree view for the multibody system
mbody_tree_view: kw.TreeView
# tree view for subtrees
subtrees_tree_view: kw.TreeView
# the visjs creation method - this needs to be fixed (TODO)
setup_visjs: Callable
# the info pane creation method - this needs to be fixed (TODO)
setup_info_panel: Callable
# visjs servers for subtree graphs
visjs_servers: dict[int, tuple[vjs.MultibodyGraphServer, bool]]
# visjs server for frames graph
visjs_frames_server: vjs.NetworkGraph
# method to create frames graph
setup_frames_visjs: Callable
# visjs iframe for all graphs
visjs_iframe: kw.IFrame
# the 3D graphics iframe
graphics_frame: kw.IFrame
# callable to signal that an error ocurred
signal_error: Callable
# background thread for long-running async callbacks
worker: AsyncWorker
[docs]
class AbstractCard(ABC, WrappedTypeMixin[T]):
"""Interface for an info card for a given item type."""
def __init__(self, context: GuiContext):
"""Create the AbstractCard.
Derived classes SHOULD call this first in their constructor.
"""
self._context = context
self._wroot = kw.Layout(
context.router, style={"display": "flex", "flexDirection": "column", "gap": "0.5em"}
)
# Add the single line header
self._wheader = kw.Markdown(context.router, text="")
self._wheader.addDomClass("karana-card-header")
self._wheader.setVisible(False)
self._wroot.addChild(self._wheader)
# Add the markdown summary block
self._wsummary = kw.Markdown(context.router, text="")
self._wsummary.addDomClass("karana-card-summary")
self._wsummary.setVisible(False)
self._wroot.addChild(self._wsummary)
@property
def item(self) -> T:
"""Get the current item or throw an error if it isn't set."""
if not hasattr(self, "_item_getter"):
raise RuntimeError("Accessed Card item before setting it")
item = self._item_getter()
if item is None:
raise RuntimeError("Card item has gone out of scope!")
return item
@item.setter
def item(self, item: T, /):
"""Set the current item for the card."""
if isinstance(item, kc.Base):
# Store the item as a weak reference to avoid causing
# issues with cleanup
self._item_getter = kc.CppWeakRef(item)
else:
# It's not a base, so just save a trivial lambda getter so
# that the other methods can assume self._item_getter()
# gets the item.
self._item_getter = lambda: item
@item.deleter
def item(self):
"""Clear the current item from the card."""
del self._item_getter
[docs]
def getItem(self) -> T | None:
"""Get the current item or None if it isn't set.
If the item was previously set but has gone out of scope,
throws an error.
"""
if not hasattr(self, "_item_getter"):
return None
item = self._item_getter()
if item is None:
# This happens if if the item has gone out of scope
# raise RuntimeError("Card item has gone out of scope!")
pass
return item
@property
def context(self) -> GuiContext:
return self._context
@property
@abstractmethod
def label(self) -> str:
"""Get a text label for this card."""
@property
def wroot(self) -> kw.Layout:
"""Get the root widget for this card."""
return self._wroot
[docs]
def teardown(self, _: T, /):
"""Do any necessary cleanup when leaving the given item.
Cards MAY override this if any cleanup is needed.
"""
[docs]
def setup(self, item: T, item_context: kw.Json, /):
"""Set up the card for the new item.
Derived Card classes SHOULD call this to update the header and
summary.
"""
header = self.getHeader()
self._wheader.setText(header)
self._wheader.setVisible(bool(header))
summary = self.getSummary()
# Cleanup any unintented whitespace and surrounding whitespace
summary = textwrap.dedent(summary).strip()
self._wsummary.setText(summary)
self._wsummary.setVisible(bool(summary))
[docs]
def updateFor(self, item: T, item_context: kw.Json, /):
"""Set the item to display and refresh.
By default this will teardown the old item and setup the new
one (which may be the same item when refreshing). Cards MAY
override this method to make optimizations.
"""
if old_item := self.getItem():
self.teardown(old_item)
self.item = item
self.setup(item, item_context)
[docs]
def close(self):
"""Do any necessary cleanup."""
[docs]
def isCompatible(self, item: Any) -> bool:
"""Check whether the Card knows how to display an item."""
result = isinstance(item, self.wrapped())
# print("AAAA", item, self.wrapped(), result)
return result
[docs]
def getSummary(self) -> str:
"""Get a markdown summary of the current item."""
# result = super().getSummary(self)
return ""
def _doTbd(self):
print("implementation TBD")
# Registry mapping item types to their card
known_card_types = []
# Class decorator that saves the card to a list of known card types
def register(cls):
assert issubclass(cls, AbstractCard)
known_card_types.append(cls)
return cls
@register
class BaseCard(AbstractCard[kc.Base]):
"""Card to display info about any Base-derived object."""
@property
def label(self) -> str:
return "Base"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wdump = kw.Markdown(router, text="")
self._wverbosity_error = kw.Toggle(
router,
text="ERROR",
on_toggle=lambda cstate: self._setVerbosity(
kc.LogLevel.ERROR
), # kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.ERROR),
tooltip="Change verbosity to ERROR level",
render_as_button=True,
)
self._wverbosity_warn = kw.Toggle(
router,
text="WARNING",
on_toggle=lambda cstate: self._setVerbosity(kc.LogLevel.WARN),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.WARN),
tooltip="Change verbosity to WARNING level",
render_as_button=True,
)
self._wverbosity_debug = kw.Toggle(
router,
text="DEBUG",
on_toggle=lambda cstate: self._setVerbosity(kc.LogLevel.DEBUG),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.DEBUG),
tooltip="Change verbosity to DEBUG level",
render_as_button=True,
)
self._wverbosity_trace = kw.Toggle(
router,
text="TRACE",
on_toggle=lambda cstate: self._setVerbosity(kc.LogLevel.TRACE),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE),
tooltip="Change verbosity to TRACE level",
render_as_button=True,
)
# Setup widget topology
# self._wmd_verbosity = kw.Markdown(router, text="**Verbosity**", in_line=True)
self._wlayout_verbosity = widgetArray(
router,
label="Verbosity",
children=[
self._wverbosity_error,
self._wverbosity_warn,
self._wverbosity_debug,
self._wverbosity_trace,
],
kind="inputgroup",
)
self.wroot.addChild(self._wlayout_verbosity)
self._wlayout_dump = kw.Layout(router)
self.wroot.addChild(self._wlayout_dump)
self._wmd_dump = kw.Markdown(router, text="**Introspection**", in_line=True)
self._wlayout_dump.addChild(self._wdump)
def _setVerbosity(self, level):
kc.MsgLogger.changeVerbosity("stdout", level)
self._wverbosity_error.setValue(level == kc.LogLevel.ERROR)
self._wverbosity_warn.setValue(level == kc.LogLevel.WARN)
self._wverbosity_debug.setValue(level == kc.LogLevel.DEBUG)
self._wverbosity_trace.setValue(level == kc.LogLevel.TRACE)
def setup(self, item: kc.Base, item_context: kw.Json, /):
# Update header/summary text
super().setup(item, item_context)
self._setVerbosity(kc.MsgLogger.getVerbosity("stdout"))
# Update the text in the dump markdown widget
dump = item.dumpString()
dump_md_template = textwrap.dedent(
"""
### `dumpString` output:
```
{dump}
```
"""
).strip()
dump_md = dump_md_template.format(dump=dump)
self._wdump.setText(dump_md)
@register
class BaseWithVarsCard(AbstractCard[kc.BaseWithVars]):
"""Card to display vars for any BaseWithVars-derived object."""
def __init__(self, context: GuiContext):
super().__init__(context)
self._wtree = kw.DataTree(context.router)
self.wroot.addChild(self._wtree)
@property
def label(self) -> str:
return "Vars"
def setup(self, item: kc.BaseWithVars, item_context: kw.Json, /):
# Update header/summary text
super().setup(item, item_context)
all_vars = item.getVars().getAllVars()
def toJson(node: kc.NestedVars | kc.Var) -> kw.Json:
if isinstance(node, kc.NestedVars):
result = {}
for leaf in node.local_vars:
result[leaf.name()] = toJson(leaf)
for branch in node.nested_vars:
result[branch.name] = toJson(branch)
return result
try:
return node.dumpString().replace("\\s+", " ").replace("\\", "")
except Exception:
msg = f"Error evaluating Var {node.name()}:\n{traceback.format_exc()}"
kc.error(msg)
self.context.signal_error()
return "ERROR"
json_data = toJson(all_vars)
self._wtree.setData(json_data)
"""
def setup(self, item: kc.BaseWithVars, item_context: kw.Json, /):
# Update header/summary text
super().setup(item, item_context)
var_paths: list[Path] = []
nodes: list[kw.TreeView.Node] = []
edges: list[kw.TreeView.Edge] = []
# Generates integer ids
id_gen = itertools.count(1)
def _extendLeaf(leaf: kc.Var, parent_path: Path, parent_id: int | None):
path = parent_path / leaf.name()
var_paths.append(path)
id_ = next(id_gen)
if quantity := leaf.quantity():
quantity = f"[{quantity}]"
try:
value = leaf.dumpString()
except Exception:
msg = f"Error evaluating Var {leaf.name()}:\n{traceback.format_exc()}"
kc.error(msg)
self.context.signal_error()
value = "ERROR"
nodes.append(
kw.TreeView.Node(
id=id_,
label=f"{leaf.name()} {value} {quantity}",
tooltip=leaf.description(),
)
)
if parent_id is not None:
edges.append(kw.TreeView.Edge(parent_id=parent_id, child_id=id_))
def _extendBranch(
branch: kc.NestedVars, parent_path: Path, parent_id: int | None, top_level=False
):
path = parent_path / branch.name
var_paths.append(path)
id_ = next(id_gen)
nodes.append(
kw.TreeView.Node(
id=id_,
label=branch.name,
tooltip=branch.description,
collapsed=not top_level,
)
)
if parent_id is not None:
edges.append(kw.TreeView.Edge(parent_id=parent_id, child_id=id_))
for leaf in branch.local_vars:
_extendLeaf(leaf, path, id_)
for ch_branch in branch.nested_vars:
_extendBranch(ch_branch, path, id_)
_extendBranch(
item.getVars().getAllVars(), parent_path=Path("/"), parent_id=None, top_level=True
)
# Check whether the topology/naming has changed
if var_paths == self._var_paths:
# Same structure so just update the labels
for node in nodes:
self._wtreeview.setNodeLabel(node.id, node.label)
else:
# Structure has changed so update the entire tree
self._var_paths = var_paths
self._wtreeview.setTree(nodes, edges)
"""
def isCompatible(self, item: Any) -> bool:
if not isinstance(item, kc.BaseWithVars):
return False
# Don't show this card for items without any vars
if item.getVars() is None:
return False
all_vars = item.getVars().getAllVars()
return bool(all_vars.local_vars or all_vars.nested_vars)
@register
class FrameCard(AbstractCard[kf.Frame]):
"""Card to display info about any Frame-derived object."""
@property
def label(self) -> str:
return "Frame"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# -------------------------
self._waxes = kw.Button(
router,
text="2d axes",
on_press=self._toggleAxes,
tooltip="Toggle line axes for the frame",
)
self._waxes3d = kw.Button(
router,
text="3d axes",
on_press=self._toggleAxes3d,
tooltip="Toggle 3D axes for the frame",
)
self._wview_around = kw.Button(
router,
text="View around",
on_press=self._viewAround,
tooltip="Re-center 3D graphics camera to view this frame",
)
# Setup widget topology
# self._wmd_highlight = kw.Markdown(router, text="**Highlight**", in_line=True)
self._wlayout_highlight = widgetArray(
router,
label="Highlight",
children=[self._waxes, self._waxes3d, self._wview_around],
kind="inputgroup",
)
# self.wroot.addChild(self._wlayout_highlight)
# --------------------
# AG - the following should be for a selected f2f insstead of
# being tied to the newtonian frame.
# Record which vizutils visualizations are active
self._lvel_cbs = {}
self._avel_cbs = {}
self._laccel_cbs = {}
self._aaccel_cbs = {}
def _toggleRatesViz(frame, rtype, color, cbsmap):
id_ = frame.id()
scene = self.context.scene
if id_ in cbsmap:
# clean up
# print("Deleting")
cbsmap[id_]()
del cbsmap[id_]
else:
# print("Creating")
dark = vizutils.visualizeFrameToFrameRates(
self.context.multibody.virtualRoot().frameToFrame(frame),
self.context.scene,
rtype,
)
dark.setRadius(0.01)
dark.setColor(color)
dark.registerCallback()
cbsmap[id_] = dark
scene.update()
# enable linear velocity visualization for the frame (only webscene)
self._wvel_linear = kw.Button(
router,
text="Linear velocity",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.VEL_LINEAR,
ks.Color.RED,
self._lvel_cbs,
),
tooltip="Enable visualization of the linear velocity of the frame with respect to the inertial frame",
)
# enable angular velocity visualization for the frame (only webscene)
self._wvel_angular = kw.Button(
router,
text="Angular velocity",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.VEL_ANGULAR,
ks.Color.GREEN,
self._avel_cbs,
),
tooltip="Enable visualization of the angular velocity of the frame with respect to the inertial frame",
)
# enable linear acceleration visualization for the frame (only webscene)
self._waccel_linear = kw.Button(
router,
text="Linear acceleration",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.ACCEL_LINEAR,
ks.Color.BLUE,
self._laccel_cbs,
),
tooltip="Enable visualization of the linear acceleration of the frame with respect to the inertial frame",
)
# enable angular acceleration visualization for the frame (only webscene)
self._waccel_angular = kw.Button(
router,
text="Angular acceleration",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.ACCEL_ANGULAR,
ks.Color.YELLOW,
self._aaccel_cbs,
),
tooltip="Enable visualization of the angular acceleration of the frame with respect to the inertial frame",
)
self._wlayout_visrates = widgetArray(
router,
label="Visualize Rates",
children=[
self._wvel_linear,
self._wvel_angular,
self._waccel_linear,
self._waccel_angular,
],
kind="inputgroup",
)
# self.wroot.addChild(self._wlayout_visrates)
# ----------
self._wlayout_viz = widgetArray(
router,
label="Visualize frame and its rates",
children=[
self._wlayout_highlight, # highlight frames
self._wlayout_visrates, # enable rates visualization
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wlayout_viz.setTooltip(
"Highlight a frame and turn on visualization of its Newtonian frame relative rates and accelerations"
)
self._wlayout_viz.setOpen(True)
self.wroot.addChild(self._wlayout_viz)
# -------------------------
def _dumpChainCB(cstate):
# print("HH", self.context.visjs_frames_server)
if cstate:
def f2fDumpAction(id):
# look up frame from id
other_frame = cast(kf.Frame, kc.BaseContainer.singleton().at(id))
print("")
self.item.frameToFrame(other_frame).dump()
print("")
# keep the highlighting on the selected frame and not
# change to the new picked frame.
self.context.effects.graph_highlighter.set(
[EffectItem(obj=cast(kc.Base, self.item), params=None)]
)
print(
"==> Pick a frame to dump the f2f from the selected frame to the picked frame"
)
self.context.visjs_frames_server.visjs_frames_noselect_cb = f2fDumpAction
# reset the other buttons
# self._wdumpf2f.setValue(False)
self._wpath.setValue(False)
self._wselectf2f.setValue(False)
else:
print("==> Returning to regular frame selection mode")
self.context.visjs_frames_server.visjs_frames_noselect_cb = None
self._wdumpf2f = kw.Toggle(
router,
text="Dump chain",
on_toggle=lambda cstate: _dumpChainCB(cstate),
tooltip="Dump information about the f2f between the selected frame and this frame",
render_as_button=True,
)
def _pathChainCB(cstate):
# print("HH", self.context.visjs_frames_server)
if cstate:
def f2fPathAction(id):
# look up frame from id
other_frame = cast(kf.Frame, kc.BaseContainer.singleton().at(id))
path = self.item.frameToFrame(other_frame).getPath()
# frames_path = [x.pframe() for x in path]
# frames_path += [x.oframe() for x in path]
_highlightGraphFrames(True, path, self.context)
# need to call this to keep the selection at this node
# _selectObject(self.item, self.context.selection),
print(
"==> Pick a frame to highlight the path between the selected and picked frames"
)
self.context.visjs_frames_server.visjs_frames_noselect_cb = f2fPathAction
# reset the other buttons
self._wdumpf2f.setValue(False)
# self._wpath.setValue(False)
self._wselectf2f.setValue(False)
else:
print("==> Returning to regular frame selection mode")
self.context.visjs_frames_server.visjs_frames_noselect_cb = None
self._wpath = kw.Toggle(
router,
text="Show path",
on_toggle=lambda cstate: _pathChainCB(cstate),
tooltip="Highlight the frames path to the next selected frame",
render_as_button=True,
)
def _selectChainCB(cstate):
# print("HH", self.context.visjs_frames_server)
if cstate:
def f2fSelectAction(id):
# look up frame from id
other_frame = cast(kf.Frame, kc.BaseContainer.singleton().at(id))
_selectObject(self.item.frameToFrame(other_frame), self.context.selection)
# reset
self._wselectf2f.setValue(False)
self.context.visjs_frames_server.visjs_frames_noselect_cb = None
print(
"==> Pick a frame to switch selection to the f2f between the selected and picked frames"
)
self.context.visjs_frames_server.visjs_frames_noselect_cb = f2fSelectAction
# reset the other buttons
self._wdumpf2f.setValue(False)
self._wpath.setValue(False)
# self._wselectf2f.setValue(False)
else:
print("==> Returning to regular frame selection mode")
self.context.visjs_frames_server.visjs_frames_noselect_cb = None
self._wselectf2f = kw.Toggle(
router,
text="Go to f2f",
on_toggle=lambda cstate: _selectChainCB(cstate),
tooltip="Change selection to the fraame to frame for the next picked frame",
render_as_button=True,
)
self._wlayout_actions = widgetArray(
router,
label="FrameToFrame chain actions (via pick mode)",
children=[self._wdumpf2f, self._wpath, self._wselectf2f],
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wlayout_actions)
self._wlayout_actions.setTooltip("Explore relationship of this frame with other frames")
# --------------------
def _firstChildFrame(f: kf.Frame):
cf = f.childrenFrames()
if len(cf) > 0:
return cf[0]
else:
return None
# change selection to the first child body (drop down, or multiple buttons)
self._wselect_down = kw.Button(
router,
text="Down",
on_press=lambda: _selectObject(
_firstChildFrame(self.item),
self.context.selection,
),
tooltip="Change selection to a frame",
)
# change selection to the parent frame
self._wselect_up = kw.Button(
router,
text="Up",
on_press=lambda: _selectObject(self.item.parentFrame(), self.context.selection),
tooltip="Change selection to the parent frame",
)
def _siblingFrame(f: kf.Frame, forward: bool):
"""Get the next/previous sibling frame."""
parent = f.parentFrame()
siblingsPlus = parent.childrenFrames()
if len(siblingsPlus) == 1:
return None
index = 0
for b in siblingsPlus:
if b.id() == f.id():
break
index += 1
nbodies = len(siblingsPlus)
next_index = (index + 1 if forward else index - 1) % nbodies
# print("index=", index, next_index)
new_frame = siblingsPlus[next_index]
return new_frame
# change selection to the next sibling body
self._wselect_right = kw.Button(
router,
text="Right",
on_press=lambda: _selectObject(_siblingFrame(self.item, True), self.context.selection),
tooltip="Change selection to a sibling frame on the right",
)
# change selection to the previous sibling body
self._wselect_left = kw.Button(
router,
text="Left",
on_press=lambda: _selectObject(_siblingFrame(self.item, False), self.context.selection),
tooltip="Change selection to a sibling frame on the left",
)
self._wlayout_select = widgetArray(
router,
label="Go to related frame",
children=[
self._wselect_up,
self._wselect_left,
self._wselect_right,
self._wselect_down,
],
kind="accordion", # "layout",
)
self._wlayout_select.setTooltip(
"Switch selection to a related frame such as the parent or a child"
)
self.wroot.addChild(self._wlayout_select)
def _toggleAxes(self):
self.context.effects.frame_axes.toggle(
[EffectItem(obj=self.item, params=AxesParams(scale=1.0, style="lines"))]
)
def _toggleAxes3d(self):
self.context.effects.frame_axes.toggle(
[EffectItem(obj=self.item, params=AxesParams(style="parts"))]
)
def _viewAround(self):
self.context.scene.viewAroundFrame(self.item, offset=[3.0, 3.0, 3.0])
def getSummary(self) -> str:
item = self.item
nd = self.context.multibody.getNodeAncestor(item)
nd_name = nd.name() if nd is not None else "(No ancestor)"
nd_str = nd.typeString() if nd is not None else "n/a"
nd_bd_str = nd.parentBody().name() if nd is not None else "n/a"
title_md = f"Anc node={nd_name} ({nd_str}) body={nd_bd_str}"
return title_md
def teardown(self, _: kf.Frame, /):
self.context.visjs_frames_server.visjs_frames_noselect_cb = None
# reset the noselect buttons
self._wdumpf2f.setValue(False)
self._wpath.setValue(False)
self._wselectf2f.setValue(False)
def setup(self, item: kf.Frame, item_context: kw.Json, /):
super().setup(item, item_context)
parent = item.parentFrame()
not_root = parent is not None
has_siblings = not_root and len(parent.childrenFrames()) > 1
has_children = len(item.childrenFrames()) > 0
self._wselect_up.setVisible(not_root)
self._wselect_right.setVisible(has_siblings)
self._wselect_left.setVisible(has_siblings)
self._wselect_down.setVisible(has_children)
@register
class FrameToFrameCard(AbstractCard[kf.FrameToFrame]):
"""Card to display info about any FrameToFrame-derived object."""
@property
def label(self) -> str:
return "FrameToFrame"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# Setup widget topology
# AG - the following should be for a selected f2f insstead of
# being tied to the newtonian frame.
# Record which vizutils visualizations are active
self._lvel_cbs = {}
self._avel_cbs = {}
self._laccel_cbs = {}
self._aaccel_cbs = {}
def _toggleRatesViz(f2f, rtype, color, cbsmap):
id_ = f2f.id()
scene = self.context.scene
if id_ in cbsmap:
# clean up
# print("Deleting")
cbsmap[id_]()
del cbsmap[id_]
else:
# print("Creating")
dark = vizutils.visualizeFrameToFrameRates(
f2f,
self.context.scene,
rtype,
)
dark.setRadius(0.01)
dark.setColor(color)
dark.registerCallback()
cbsmap[id_] = dark
scene.update()
# enable linear velocity visualization for the frame (only webscene)
self._wvel_linear = kw.Button(
router,
text="Linear velocity",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.VEL_LINEAR,
ks.Color.RED,
self._lvel_cbs,
),
tooltip="Visualize the relative linear velocity across this frame to frame.",
)
# enable angular velocity visualization for the frame (only webscene)
self._wvel_angular = kw.Button(
router,
text="Angular velocity",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.VEL_ANGULAR,
ks.Color.GREEN,
self._avel_cbs,
),
tooltip="Visualize the relative angular velocity across this frame to frame.",
)
# enable linear acceleration visualization for the frame (only webscene)
self._waccel_linear = kw.Button(
router,
text="Linear acceleration",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.ACCEL_LINEAR,
ks.Color.BLUE,
self._laccel_cbs,
),
tooltip="Visualize the relative linear acceleration across this frame to frame.",
)
# enable angular acceleration visualization for the frame (only webscene)
self._waccel_angular = kw.Button(
router,
text="Angular acceleration",
on_press=lambda: _toggleRatesViz(
self.item,
vizutils.FrameToFrameRateType.ACCEL_ANGULAR,
ks.Color.YELLOW,
self._aaccel_cbs,
),
tooltip="Visualize the relative angular acceleration across this frame to frame.",
)
def _pathFrames(f2f: kf.FrameToFrame) -> list[kf.Frame]:
if isinstance(f2f, kf.EdgeFrameToFrame):
return [f2f.oframe(), f2f.pframe()]
elif isinstance(f2f, kf.OrientedChainedFrameToFrame):
of2f = cast(kf.OrientedChainedFrameToFrame, f2f)
return of2f.getPath()
elif isinstance(f2f, kf.ChainedFrameToFrame):
cf2f = cast(kf.ChainedFrameToFrame, f2f)
return cf2f.getPath()
else:
raise ValueError(
f"Cannot get pathFrames for {f2f.name()} of type {f2f.typeString()}"
)
# change selection to the first child body (drop down, or multiple buttons)
self._wpath = kw.Toggle(
router,
text="Frames path",
on_toggle=lambda cstate: _highlightFrames2(
_pathFrames(self.item), cstate, gui_context=self.context, priority_level="primary"
),
tooltip="Highlight the frames path connecting the oframe to the pframe",
render_as_button=True,
# addBorder=True,
)
# self.wroot.addChild(self._wpath)
# self._wmd_visualizerates = kw.Markdown(router, text="**Visualize Rates**", in_line=True)
self._wlayout_visrates = widgetArray(
router,
label="Rates",
children=[
self._wvel_linear,
self._wvel_angular,
self._waccel_linear,
self._waccel_angular,
],
kind="layout", # "inputgroup",
)
self._wlayout_viz = widgetArray(
router,
label="Visualization",
children=[
self._wpath,
self._wlayout_visrates,
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wlayout_viz.setTooltip(
"Visualization options for the frame to frame such as highlighting the connection frames path, and visualizing its relative velocitities and accelerations."
)
self._wlayout_viz.setOpen(True)
self.wroot.addChild(self._wlayout_viz)
# --------------------
# change selection to the pframe
self._wselect_pframe = kw.Button(
router,
text="Go to pframe",
on_press=lambda: _selectObject(self.item.pframe(), self.context.selection),
tooltip="Change selection to the (to) pframe for this frame to frame",
)
# change selection to the oframe
self._wselect_oframe = kw.Button(
router,
text="Go to oframe",
on_press=lambda: _selectObject(self.item.oframe(), self.context.selection),
tooltip="Change selection to the (from) oframe for this frame to frame",
)
"""
self._wmd_highlight = kw.Markdown(router, text="**Highlight**", in_line=True)
self._wlayout_highlight = widgetArray(
router,
label=self._wmd_highlight,
children=[self._waxes, self._waxes3d, self._wview_around],
)
self.wroot.addChild(self._wlayout_highlight)
"""
# self._wmd_selection = kw.Markdown(router, text="**Selection**", in_line=True)
self._wlayout_select = widgetArray(
router,
label="Switch to the from/to frames",
children=[
self._wselect_oframe,
self._wselect_pframe,
],
kind="accordion", # "inputgroup",
)
self._wlayout_select.setTooltip(
"Switch selection to the oframe or pframe for this frame to frame"
)
self.wroot.addChild(self._wlayout_select)
def setup(self, item: kf.FrameToFrame, item_context: kw.Json, /):
super().setup(item, item_context)
has_path = isinstance(item, kf.OrientedChainedFrameToFrame) or isinstance(
item, kf.ChainedFrameToFrame
)
self._wpath.setVisible(has_path)
def _firstChildBody(st: kd.SubTree, bd: kd.BodyBase) -> kd.BodyBase | None:
"""Return the first child body if the body has children bodies."""
children = st.childrenBodies(bd)
child = None if not children else children[0]
return child
def _parentBody(st: kd.SubTree, bd: kd.BodyBase) -> kd.BodyBase | None:
"""Return the parent body if the body has one."""
return None if st.isBaseBody(bd) else st.parentBody(bd)
def _siblingNode(nd: kd.Node, forward: bool):
"""Get the next/previous sibling node."""
bd = nd.parentBody()
siblingsPlus = bd.nodeList()
if len(siblingsPlus) == 1:
return None
index = 0
for n in siblingsPlus:
if n.id() == nd.id():
break
index += 1
nbodies = len(siblingsPlus)
next_index = (index + 1 if forward else index - 1) % nbodies
# print("index=", index, next_index)
new_node = siblingsPlus[next_index]
return new_node
def _siblingBody(st: kd.SubTree, bd: kd.BodyBase, forward: bool) -> kd.BodyBase | None:
"""Get the next/previous sibling body."""
parent = st.parentBody(bd)
siblingsPlus = st.childrenBodies(parent)
if len(siblingsPlus) == 1:
return None
index = 0
for b in siblingsPlus:
if b.id() == bd.id():
break
index += 1
nbodies = len(siblingsPlus)
next_index = (index + 1 if forward else index - 1) % nbodies
# print("index=", index, next_index)
new_body = siblingsPlus[next_index]
return new_body
def _firstChildSubtree(st: kd.SubTree) -> kd.SubTree | None:
"""Return the first child subtree if the subtree has children bodies."""
children = st.childrenSubTrees()
child = None if not children else children[0]
return child
def _parentSubtree(st: kd.SubTree) -> kd.SubTree | None:
"""Return the parent subtree if the subtree has one."""
return st.parentSubTree()
def _siblingSubtree(st: kd.SubTree, forward: bool) -> kd.SubTree | None:
"""Get the next/previous sibling subtree."""
parent = st.parentSubTree()
if not parent:
return None
siblingsPlus = parent.childrenSubTrees()
if len(siblingsPlus) == 1:
return None
index = 0
for b in siblingsPlus:
if b.id() == st.id():
break
index += 1
nsts = len(siblingsPlus)
next_index = (index + 1 if forward else index - 1) % nsts
# print("index=", index, next_index)
new_subtree = siblingsPlus[next_index]
return new_subtree
# callback to create a SubTree's tree view
def _createSubTreeView(st, gui_context):
# if isinstance(st, kd.Multibody):
# return
from ._treeview import createSubTreeBodiesTreeView
tv = createSubTreeBodiesTreeView(
router=gui_context.router,
gui_selection_state=gui_context.selection,
st=st,
)
tv.refresh()
gui_context.dock.addChild(
title=f"{st.name()}",
widget=tv,
relative_to=gui_context.mbody_tree_view,
direction="within",
)
# callback to change subhinge coords set by the sliders
def changeSubhingeCoord(item, st, scene, Q, shindex, cindex, with_ik, wstatus=None):
if not item:
return
if shindex is None:
print("WARNING: Please select a coordinate first ...")
return
if isinstance(item, kd.PhysicalBody):
hge = item.parentHinge()
elif isinstance(item, kd.LoopConstraintCutJoint):
hge = item.hinge()
elif isinstance(item, kd.FramePairHinge):
hge = item
elif isinstance(item, kd.CompoundBody):
hge = item.parentHinge()
elif isinstance(item, kd.CompoundHinge):
hge = item
else:
raise ValueError(f"Not sure what to do with item {item}.")
if not shindex < hge.nSubhinges():
raise ValueError(
f"The {shindex} subhinge index should be less than {hge.nSubhinges()} - the number of subhinges for the {hge.name()} hinge"
)
sh = hge.subhinge(shindex)
if not cindex < sh.nQ():
raise ValueError(
f"The {cindex} coordinate index should be less than {sh.nQ()} - the number of coordinates for the {sh.name()} subhinge"
)
Qvec = sh.getQ()
Qvec[cindex] = Q
sh.setQ(Qvec)
if isinstance(st, kd.SubGraph) and with_ik:
# do IK
# offset = st.coordOffsets(sh).Q
st.cks().freezeCoord(sh, cindex, kd.CKFrozenCoordType.Q)
err = st.cks().solveQ()
if err < 1e-10:
color = "green"
stxt = "SUCCESS"
extra = f"[Q={Q:.4}]"
else:
color = "red"
stxt = "FAILED"
extra = f"[Q={Q:.4}, err={err:.4e}]"
status = f'**IK status:** <span style="color:{color}">{stxt}</span> {extra}'
# print(" err=", err, color)
st.cks().unfreezeCoord(sh, cindex, kd.CKFrozenCoordType.Q)
else:
status = "**IK status:** N/A"
if wstatus:
wstatus.setText(status)
scene.update()
def _createSubTreeVisJs(st: kd.SubTree, gui_context: GuiContext, use_id_labels: bool = False):
"""Create a new tab with visjs layout for the subtree."""
server = gui_context.setup_visjs(st, use_id_labels)
# gui_context.effects.registerGraphServer(server)
gui_context.visjs_servers[st.id()] = (server, True)
iframe = kw.IFrame(gui_context.router, server.getUrl())
gui_context.dock.addChild(st.name(), iframe, gui_context.visjs_iframe, "within")
if isinstance(st, kd.SubGraph):
if len(st.enabledConstraints()) > 0:
server.enableSubGraph("constraints")
def _createFramesVisJs(use_id_labels: bool, gui_context: GuiContext):
"""Create a new tab with visjs layout for the frames."""
server = gui_context.setup_frames_visjs(gui_context.multibody.frameContainer(), use_id_labels)
# gui_context.effects.registerGraphServer(server)
gui_context.visjs_frames_server = server
iframe = kw.IFrame(gui_context.router, server.getUrl())
nm = "frames" if use_id_labels else "frames_ids"
gui_context.dock.addChild(nm, iframe, gui_context.visjs_iframe, "within")
def _highlightBodyConstraints(body: kd.PhysicalBody, gui_context: GuiContext):
"""Show a line between this body and other bodies with which it has constraints."""
# Get a list of all constraints including this body
constraints = gui_context.multibody.getBodyLoopConstraints(body)
effects = []
for constraint in constraints:
if sn := constraint.sourceNode():
src_body = sn.parentBody()
else:
raise ValueError(f"Cannot get source node from constraint {constraint.name()}")
if tn := constraint.targetNode():
tgt_body = tn.parentBody()
else:
raise ValueError(f"Cannot get target node from constraint {constraint.name()}")
params = GraphEdgeParams(
color="#00ff00",
dashed=True,
id=constraint.id(),
arrows=True,
title=f"'{constraint.name()}' ({constraint.typeString()}) constraint between\n the '{src_body.name()}/{tgt_body.name()}'\n bodies",
)
effect = EffectItem(obj=(src_body, tgt_body), params=params)
effects.append(effect)
gui_context.effects.graph_edge_adder.toggle(effects)
def recreateStickParts(st: kd.SubTree, gui_context: GuiContext):
st.removeStickParts()
st.multibody().createStickParts()
_toggleVisibleBodies(
True, st.sortedPhysicalBodiesList(), gui_context, layers=ks.LAYER_STICK_FIGURE
)
def _highlightBodyNodes(cstate: bool, bodies: list[kd.PhysicalBody], effects: EffectManager):
"""Toggle frames axes for nodes on a physical bodies."""
if cstate:
axes_effects = []
axes_params = AxesParams()
for body in bodies:
# toggle axes for the nodes
for node in body.nodeList():
axes_effects.append(EffectItem(obj=node, params=axes_params))
effects.frame_axes.set(axes_effects)
else:
effects.frame_axes.set([])
def _showLoopConstraints(lcs: list[kd.LoopConstraintBase], gui_context: GuiContext):
"""Show loop constraints involving the pair of bodies in visjs."""
# TBD AG - show line in visjs connecting the pair of bodies for this loop constraint
assert 0
"""
mb = gui_context.multibody
for c in lcs:
snd = c.sourceNode()
tnd = c.targetNode()
bd1 = mb.virtualRoot() if not snd else snd.parentBody()
bd2 = mb.virtualRoot() if not tnd else tnd.parentBody()
_showConstraint(bd1, bd2)coo
pass
"""
def _swingHinge(
st: kd.SubTree, hinges: list[kd.HingeBase | None], disable_ik: bool, gui_context: GuiContext
):
"""Articulate bodies in WebScene sequentially."""
for hinge in hinges:
# Loop through and articulate each coordinate associated with a FramePairHinge
if isinstance(hinge, kd.FramePairHinge):
axes_params = AxesParams()
outline_params = OutlineParams(level="secondary")
for coord_offset in range(hinge.coordData().nU()):
subhinge, subhinge_offset = hinge.coordData().coordAt(coord_offset)
sh = cast(kd.PhysicalSubhinge, subhinge)
pfrm = sh.pframe()
axes_effects = [EffectItem(obj=pfrm, params=axes_params)]
bd = gui_context.multibody.getNodeAncestor(pfrm).parentBody()
outline_effects = [
EffectItem(
obj=bd, # cast(kf.Frame, body.pnode().parentBody()),
params=outline_params,
)
]
gui_context.effects.frame_axes.toggle(axes_effects)
gui_context.effects.frame_outliner.toggle(outline_effects)
st.articulateSubhinge(sh, subhinge_offset, disable_ik)
gui_context.effects.frame_axes.toggle(axes_effects)
gui_context.effects.frame_outliner.toggle(outline_effects)
def _highlightFrames(
cstate: bool,
frames: Sequence[kf.Frame], # toggle_over_set: bool,
gui_context: GuiContext,
):
"""Highlight frame axes."""
if cstate:
axes_effects = []
axes_params = AxesParams()
for f in frames:
axes_effects.append(EffectItem(obj=f, params=axes_params))
if 0: # and toggle_over_set:
gui_context.effects.frame_axes.toggle(axes_effects)
else:
gui_context.effects.frame_axes.set(axes_effects)
else:
gui_context.effects.frame_axes.set([])
def _highlightFrames2(
frames: Sequence[kf.Frame], cstate: bool, gui_context: GuiContext, priority_level: str
):
"""Highlight frame axes."""
axes = []
scale = 0
if cstate:
if priority_level == "primary":
scale = 1
elif priority_level == "secondary":
scale = 0.75
elif priority_level == "tertiary":
scale = 0.5
params = AxesParams(scale=scale)
for f in frames:
axes.append(EffectItem(obj=f, params=params))
gui_context.effects.frame_axes.ensure(axes)
_highlightGraphFrames(True, frames, gui_context)
def _getPhysicalBodies(bdlist: Sequence[kd.BodyBase]) -> list[kd.PhysicalBody]:
result = []
for bd in bdlist:
if not bd.isCompoundBody():
result.append(bd)
else:
result.extend(cast(kd.CompoundBody, bd).physicalBodiesTree().sortedPhysicalBodiesList())
return result
# ------------------------------------
# mbody level kinematics sim callback
def _subTreeSim(
st: kd.SubTree,
integ_type: int,
gravity_accel: NDArray[np.float64] | None,
contact_force: kcoll.ContactForceBase | None,
gui_context: GuiContext,
):
# create an SP and put it in kinematics mode
sp = kd.StatePropagator.create(
st, ki.IntegratorType(integ_type), None, None, kd.MMSolverType.FORWARD_DYNAMICS
)
# freeze coord
# st.setU(init_U)
if isinstance(st, kd.SubGraph) and st.enabledConstraints():
st.cks().solveU()
ti = 0
sp.setTime(ti)
x = sp.assembleState()
sp.setState(x)
# add a limit of hop size to force smooth visualization updates
sp.setMaxHopSize(0.01)
kmdl.UpdateProxyScene.create("scene", sp, gui_context.scene)
kmdl.TimeDisplay.create("time_display", sp, gui_context.scene.graphics())
kmdl.SyncRealTime.create("sync_real", sp, 1.0)
if gravity_accel is not None:
ug = kmdl.Gravity("grav_model", sp, kmdl.UniformGravity("uniform_gravity"), st)
ug.getGravityInterface().setGravity(
np.array([0, 0, -3.73]), 0.0, kmdl.OutputUpdateType.PRE_HOP
)
if contact_force is not None:
col_scene = ks.CoalScene("collision_scene")
fcoll = kcoll.FrameCollider(gui_context.scene, col_scene)
gui_context.scene.registerClientScene(
col_scene, gui_context.multibody.virtualRoot(), layers=ks.LAYER_COLLISION
)
for bd1 in st.sortedPhysicalBodiesList():
bd2 = bd1.onode().parentBody()
# ignore all inter-body collisions
fcoll.ignoreFramePair(bd1, bd2)
fcoll.ignoreAllCurrentlyTouchingPairs()
kmdl.PenaltyContact(
"penalty_contact",
sp,
st,
[fcoll],
contact_force,
)
"""
# run a loop to advance time by
# sp.advanceBy(duration)
for i in range(10):
print(f"Advancing to {sp.getTime()} ...")
sp.advanceBy(duration / 10)
"""
return sp
# ------------------------------------
# mbody level kinematics sim callback
def _mbodyKinematicsSim(
sh: kd.SubhingeBase, cindex: int, deltaq: float, duration: float, gui_context: GuiContext
):
# shindex = self._coord_move_indices[0]
# # get the selected sindex/cindex, and the Q values
# if shindex is None:
# return
# cindex = self._coord_move_indices[1]
# hge = self.item.parentHinge()
# sh = hge.subhinge(shindex)
# assert kc.allReady()
if not kc.allReady():
print("WARNING: kc.allReady() is failing - call mb.resetData()")
return
# create an SP and put it in kinematics mode
sg = cast(kd.SubGraph, gui_context.multibody)
sp = kd.StatePropagator.create(
sg, ki.IntegratorType.EULER, None, None, kd.MMSolverType.KINEMATICS
)
# freeze coord
sg.cks().clearFrozenCoords()
sg.setU(0)
sg.setUdot(0)
sg.cks().freezeCoord(sh, cindex)
ti = 0
tf = duration
qi = sh.getQ()[cindex]
qf = qi + deltaq
ui = uf = 0
sp.setTime(ti)
x = sp.assembleState()
sp.setState(x)
# create a profile generator
pg = kmdl.FloatCubicSplineProfileGenerator.create("kinpg", ti, qi, ui, tf, qf, uf)
# defined pre deriv CB for setting values
def getNewCoord(t: SupportsFloat | SupportsIndex | float, _: ArrayLike):
udot = pg.getUdot(t)
# regular non-convel subhinge
udotvec = np.zeros(sh.nU())
udotvec[cindex] = udot
sh.setUdot(udotvec)
sp.fns.pre_deriv_fns["newcoord"] = getNewCoord
kmdl.UpdateProxyScene.create("scene", sp, gui_context.scene)
kmdl.TimeDisplay.create("time_display", sp, gui_context.scene.graphics())
kmdl.SyncRealTime.create("sync_real", sp, 1.0)
# sp.registerModel(update_scene)
# run a loop to advance time by
# sp.advanceBy(duration)
for _ in range(10):
print(f"Advancing to {sp.getTime()} ...")
sp.advanceBy(duration / 10)
# unfreeze coord
sg.cks().unfreezeCoord(sh, cindex)
# cegraph level kinematics sim callback
def _ceKinematicsSim(
sg: kd.SubGraph,
sh: kd.SubhingeBase,
cindex: int,
deltaq: float,
duration: float,
gui_context: GuiContext,
):
# shindex = self._coord_move_indices[0]
# # get the selected sindex/cindex, and the Q values
# if shindex is None:
# return
# assert kc.allReady()
if not kc.allReady():
print("WARNING: kc.allReady() is failing - call mb.resetData()")
return
# cindex = self._coord_move_indices[1]
# hge = self.item.parentHinge()
# sh = hge.subhinge(shindex)
# # create an SP and put it in kinematics mode
# sg = cast(kd.SubGraph, self.context.multibody)
sp = kd.StatePropagator.create(
sg, ki.IntegratorType.EULER, None, None, kd.MMSolverType.KINEMATICS
)
"""
# freeze coord
sg.cks().clearFrozenCoords()
sg.cks().freezeCoord(sh, cindex)
"""
sg.setU(0)
sg.setUdot(0)
ti = 0
tf = duration
qi = sh.getQ()[cindex]
qf = qi + deltaq
ui = uf = 0
sp.setTime(ti)
x = sp.assembleState()
sp.setState(x)
# create a profile generator
pg = kmdl.FloatCubicSplineProfileGenerator.create("kinpg", ti, qi, ui, tf, qf, uf)
kmdl.TimeDisplay.create("time_display", sp, gui_context.scene.graphics())
kmdl.SyncRealTime.create("sync_real", sp, 1.0)
# defined pre deriv CB for setting values
def getNewCoord(t: SupportsFloat | SupportsIndex | float, _: ArrayLike):
udot = pg.getUdot(t)
# regular non-convel subhinge
udotvec = np.zeros(sh.nU())
udotvec[cindex] = udot
sh.setUdot(udotvec)
sp.fns.pre_deriv_fns["newcoord"] = getNewCoord
kmdl.UpdateProxyScene.create("scene", sp, gui_context.scene)
# run a loop to advance time by
# sp.advanceBy(duration)
for _ in range(10):
print(f"Advancing to {sp.getTime()} ...")
sp.advanceBy(duration / 10)
# unfreeze coord
# sg.cks().unfreezeCoord(sh, cindex)
def _highlightGraphFrames(
cstate: bool,
frames: Sequence[kf.Frame],
gui_context: GuiContext,
):
"""Highlight frames in visjs and outline in WebScene."""
if cstate:
# print("GGGG", [x.name() for x in frames])
gui_context.effects.graph_highlighter.set(
[EffectItem(obj=cast(kc.Base, frame), params=None) for frame in frames]
)
else:
gui_context.effects.graph_highlighter.set([])
def _highlightBodies(
cstate: bool,
bodies: Sequence[kd.BodyBase], # primary
secondary_bodies: list[kd.BodyBase],
tertiary_bodies: list[kd.BodyBase],
# st: kd.SubTree,
# toggle_over_set: bool,
gui_context: GuiContext,
):
"""Highlight bodies in visjs and outline in WebScene."""
if cstate:
# do outline effects in 3D graphics
outline_effects = []
# for bd in bodies:
for bd in bodies: # _getPhysicalBodies(bodies):
if isinstance(bd, kf.Frame):
outline_effects.append(EffectItem(obj=bd, params=OutlineParams(level="primary")))
for bd in _getPhysicalBodies(bodies):
outline_effects.append(EffectItem(obj=bd, params=OutlineParams(level="primary")))
for bd in secondary_bodies: # _getPhysicalBodies(secondary_bodies):
if isinstance(bd, kf.Frame):
outline_effects.append(EffectItem(obj=bd, params=OutlineParams(level="secondary")))
for bd in _getPhysicalBodies(secondary_bodies):
outline_effects.append(EffectItem(obj=bd, params=OutlineParams(level="secondary")))
for bd in tertiary_bodies: # _getPhysicalBodies(tertiary_bodies):
if isinstance(bd, kf.Frame):
outline_effects.append(EffectItem(obj=bd, params=OutlineParams(level="tertiary")))
for bd in _getPhysicalBodies(tertiary_bodies):
outline_effects.append(EffectItem(obj=bd, params=OutlineParams(level="tertiary")))
# Now also highlight those bodies in the visjs graph. Here we don't
# have a notion of a 'level' so just highlight all of them equally.
if isinstance(bodies, list):
all_bodies = bodies + secondary_bodies + tertiary_bodies
else:
all_bodies = [x for x in bodies] + secondary_bodies + tertiary_bodies
if False: # and toggle_over_set:
gui_context.effects.graph_highlighter.toggle(
[EffectItem(obj=bd, params=None) for bd in all_bodies]
)
gui_context.effects.frame_outliner.toggle(outline_effects)
else:
gui_context.effects.graph_highlighter.set(
[EffectItem(obj=cast(kc.Base, bd), params=None) for bd in all_bodies]
)
gui_context.effects.frame_outliner.set(outline_effects)
else:
gui_context.effects.graph_highlighter.set([])
gui_context.effects.frame_outliner.set([])
def _wireframeBodies(bodies: list[kd.PhysicalBody]):
"""Change all bodies' scene parts in WebScene to wireframe mode."""
for bd in bodies:
for sp in bd.getSceneParts():
_wireframe(sp)
def _transparentBodies(bodies: list[kd.PhysicalBody]):
"""Change all bodies' scene parts in WebScene to semi-transparent mode."""
for bd in bodies:
for sp in bd.getSceneParts():
_transparent(sp)
def _wireframe(sp: ks.ScenePart):
"""Toggle wireframe mode for a scene part."""
# TBD AG - add wireframe support
assert sp
pass
def _transparent(sp: ks.ScenePart):
"""Toggle transparent mode for a scene part."""
# TBD AG - add transparency support
assert sp
pass
def _toggleVisibleBodies(cstate: bool, bodies: list[kd.PhysicalBody], context: GuiContext, layers):
"""Toggle visibility of bodies' scene parts in WebScene."""
if not cstate:
# hide the body's scene parts in the specified layer
context.effects.body_part_hider.ensure(
EffectItem(obj=(body, layers), params=None) for body in bodies
)
else:
# show the body's scene parts in the specified layer
context.effects.body_part_hider.ensureRemoved((body, layers) for body in bodies)
def _selectObject(obj: kc.Base | None, selection: kw.State, st_context=None):
"""Change the selection to the specified object."""
if obj is None:
return
print(f"Switching to '{obj.name()}' ({obj.typeString()}/{obj.id()})")
selection.set(kw.Selection([kw.Selection.Item(obj.id(), context=st_context)]).dump())
@register
class BodyBaseCard(AbstractCard[kd.BodyBase]):
"""Card to display info about any BodyBase-derived object."""
@property
def label(self) -> str:
return "BodyBase"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
# self._wnd_header = kw.Markdown(router, "---")
# self.wroot.addChild(self._wnd_header)
self._wbb_header1 = kw.Markdown(router, "**BodyBase**")
self.wroot.addChild(self._wbb_header1)
# ----------------------------
# highlight parent body, children body (different highlighting)
# pbd = cast(kd.PhysicalBody, self.item)
self._wbb_parentchildren = kw.Toggle(
router,
text="Parent/Children",
# on_press= lambda: _parentChildeCB(self.item)
on_toggle=lambda cstate: _highlightBodies(
cstate,
bodies=[self.item],
secondary_bodies=[
self.subtree.parentBody(self.item)
], # self.item.physicalParentBody()],
tertiary_bodies=[
cast(kd.PhysicalBody, x) for x in self.subtree.childrenBodies(self.item)
],
# toggle_over_set=False,
gui_context=self.context,
),
tooltip="Toggle the highlighting of the parent and children bodies",
render_as_button=True,
)
# highlight downstream bodies
def _downstreamBodies(st: kd.SubTree, bd: kd.BodyBase) -> list[kd.BodyBase]:
"""Return the physical downstream bodies for a body within a subtree."""
# print("KKKKK", [x.name() for x in st.filteredBodies(bd)])
return st.filteredBodies(bd)
self._wbb_downstream = kw.Toggle(
router,
text="Downstream",
on_toggle=lambda cstate: _highlightBodies(
cstate,
bodies=[self.item],
secondary_bodies=_downstreamBodies(self.subtree, self.item),
tertiary_bodies=[],
# toggle_over_set=False,
gui_context=self.context,
),
tooltip="Toggle the highlighting of the downstream bodies",
render_as_button=True,
)
# highlight upstream bodies
def _upstreamBodies(st: kd.SubTree, bd: kd.BodyBase) -> list[kd.BodyBase]:
"""Return the physical upstream bodies for a body."""
if st.isBaseBody(bd):
return []
else:
return st.filteredBodies(
st.virtualRoot(), [bd], [st.parentBody(bd)]
) # [bd.physicalParentBody()])
self._wbb_upstream = kw.Toggle(
router,
text="Upstream",
on_toggle=lambda cstate: _highlightBodies(
cstate,
bodies=[self.item],
secondary_bodies=_upstreamBodies(self.subtree, self.item),
tertiary_bodies=[],
# toggle_over_set=False,
gui_context=self.context,
),
tooltip="Toggle the highlighting of the upstream bodies",
render_as_button=True,
)
# self._wbb_md_highlight = kw.Markdown(router, text="**Highlight**", in_line=True)
self._wbb_layout_highlight = widgetArray(
router,
label="**Highlight**",
children=[self._wbb_parentchildren, self._wbb_upstream, self._wbb_downstream],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wbb_layout_highlight)
# ----------------------------
# articulate parent
self._wbb_swing_parent = kw.Button(
router,
text="Parent",
on_press=lambda: _swingHinge(
self.subtree, [self.item.physicalParentBody().parentHinge()], True, self.context
),
tooltip="Auto articulate the parent body",
)
# articulate childrent sequentially
self._wbb_swing_children = kw.Button(
router,
text="Children",
on_press=lambda: _swingHinge(
self.subtree,
[x.parentHinge() for x in self.subtree.childrenBodies(self.item)],
True,
self.context,
),
tooltip="Auto articulate the children bodies sequentially",
)
# articulate downstream bodies sequentially
self._wbb_swing_downstream = kw.Button(
router,
text="Downstream",
on_press=lambda: _swingHinge(
self.subtree,
[x.parentHinge() for x in _downstreamBodies(self.subtree, self.item)],
False,
self.context,
),
tooltip="Auto articulate the downstream bodies sequentially",
)
# self._wbb_md_articulate = kw.Markdown(router, text="**Articulate**", in_line=True)
self._wbb_layout_articulate = widgetArray(
router,
label="**Articulate**",
children=[self._wbb_swing_children, self._wbb_swing_parent, self._wbb_swing_downstream],
kind="layout",
)
# self.wroot.addChild(self._wbb_layout_articulate)
self._wbb_layout_nbhd = widgetArray(
router,
label="Explore neighborhood bodies",
children=[self._wbb_layout_highlight, self._wbb_layout_articulate],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wbb_layout_nbhd.setTooltip(
"Highlight and articulate neighboring bodies to explore structure"
)
self._wbb_layout_nbhd.setOpen(True)
self.wroot.addChild(self._wbb_layout_nbhd)
# ----------------------------
# change selection to the first child body (drop down, or multiple buttons)
self._wbb_select_down = kw.Button(
router,
text="Down",
on_press=lambda: _selectObject(
cast(kc.Base | None, _firstChildBody(self.subtree, self.item)),
self.context.selection,
st_context={"subtree_id": self.subtree.id()},
),
tooltip="Change selection to a child body",
)
# change selection to the parent body
self._wbb_select_up = kw.Button(
router,
text="Up",
on_press=lambda: _selectObject(
cast(kc.Base | None, _parentBody(self.subtree, self.item)),
self.context.selection,
st_context={"subtree_id": self.subtree.id()},
),
tooltip="Change selection to the parent body",
)
# change selection to the next sibling body
self._wbb_select_right = kw.Button(
router,
text="Right",
on_press=lambda: _selectObject(
cast(kc.Base | None, _siblingBody(self.subtree, self.item, True)),
self.context.selection,
st_context={"subtree_id": self.subtree.id()},
),
tooltip="Change selection to the next sibling body",
)
# change selection to the previous sibling body
self._wbb_select_left = kw.Button(
router,
text="Left",
on_press=lambda: _selectObject(
cast(kc.Base | None, _siblingBody(self.subtree, self.item, False)),
self.context.selection,
st_context={"subtree_id": self.subtree.id()},
),
tooltip="Change selection to the previous sibling body",
)
# Setup widget topology
# self._wbb_md_selection = kw.Markdown(router, text="**Selection**", in_line=True)
self._wbb_layout_select = widgetArray(
router,
label="Go to other related body",
children=[
self._wbb_select_up,
self._wbb_select_left,
self._wbb_select_right,
self._wbb_select_down,
],
kind="accordion", # "layout",
)
self._wbb_layout_select.setTooltip(
"Switch selection to a related body such as the parent, a child or a sibling body"
)
self.wroot.addChild(self._wbb_layout_select)
def getSummary(self) -> str:
result = super().getSummary()
item = self.item
if item.isRootBody():
result += f"""
Subtree: {self.subtree.name()}<br/>
Is the root body
"""
else:
hge_type = kd.HingeBase.hingeTypeString(item.parentHinge().hingeType())
result += f"""
Subtree: '{self.subtree.name()}',
parent body: '{self.subtree.parentBody(item).name()}'</br>
hinge: {hge_type}
"""
return result
def setup(self, item: kd.BodyBase, item_context: kw.Json, /):
# need to set this for 'subtree' to work
if isinstance(item_context, dict):
# set the context subtree
st_id = item_context.get("subtree_id", None)
if not isinstance(st_id, int):
raise ValueError("Could not get the subtree ID.")
self._subtree = cast(kd.SubTree, kc.BaseContainer.singleton().at(st_id))
if not isinstance(self._subtree, kd.SubTree):
raise ValueError(f"Did not get a SubTree from ID {st_id}.")
elif item and not item_context:
# assert 0 # should never get here
if not item.isCompoundBody():
kc.warn(
f"The subtree value is missing when selecting the {item.name()} body. Defaulting to multibody."
)
self._subtree = self.context.multibody
else:
kc.warn(
f"The subtree value is missing when setting up the {item.name()} compound body."
)
super().setup(item, item_context)
not_root_body = not item.isRootBody()
not_basebody = not_root_body and not self.subtree.isBaseBody(item)
has_children = not_root_body and len(self.subtree.childrenBodies(item)) > 0
if not_root_body:
has_siblings = (
not_root_body
and len(self.subtree.childrenBodies(self.subtree.parentBody(item))) > 1
)
else:
has_siblings = False
self._wbb_upstream.setVisible(not_basebody)
self._wbb_swing_parent.setVisible(not_basebody)
self._wbb_select_up.setVisible(not_basebody)
self._wbb_downstream.setVisible(has_children)
self._wbb_swing_downstream.setVisible(has_children)
self._wbb_swing_children.setVisible(has_children)
self._wbb_select_down.setVisible(has_children)
self._wbb_select_right.setVisible(has_siblings)
self._wbb_select_left.setVisible(has_siblings)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
if isinstance(item, kd.PhysicalBody) or isinstance(item, kd.CompoundBody):
return False
return isinstance(item, self.wrapped())
@property
def subtree(self):
# AG - needs to be updated to return true subgraph context
# return self.context.multibody
if self.item.isCompoundBody():
return cast(kd.CompoundBody, self.item).bodiesTree().parentSubTree()
else:
return self.context.multibody
def close(self):
if hasattr(self, "_subtree"):
del self._subtree
@register
class PhysicalBodyCard(BodyBaseCard): # AbstractCard[kd.PhysicalBody]):
"""Card to display info about any PhysicalBody-derived object."""
@property
def label(self) -> str:
return "Body"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wheader = kw.Markdown(router, "---")
self.wroot.addChild(self._wheader)
self._wheader1 = kw.Markdown(router, "**PhysicalBody**")
self.wroot.addChild(self._wheader1)
# ---------------------------------
# add articulation widgets
self._frame_pair_widgets = FramePairHingeWidgets(
self.context, group_tag=f"{self.wroot.domId()}-change_coord-group"
)
self.wroot.addChild(self._frame_pair_widgets._wik_layout)
self._frame_pair_widgets._wik_layout.setOpen(True)
# ---------------------------------
# show/hide graphics mesh scene parts (only webscene)
self._wpbd_mesh = kw.Toggle(
router,
text="Mesh",
on_toggle=lambda cstate: _toggleVisibleBodies(
cstate, [self.item], self.context, ks.LAYER_PHYSICAL_GRAPHICS
),
tooltip="Toggle visibility of mesh scene parts for the body in 3D graphics",
render_as_button=True,
)
# show/hide collision scene parts (only webscene)
self._wpbd_collision = kw.Toggle(
router,
text="Collision parts",
on_toggle=lambda cstate: _toggleVisibleBodies(
cstate, [self.item], self.context, ks.LAYER_COLLISION
),
tooltip="Toggle visibility of collision scene parts for the body in 3D graphics",
render_as_button=True,
)
# turn on/off stick parts for just the bodies in the subtree
def stickCB(cstate, bd):
has_stick = len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0
if not has_stick:
mb = self.context.multibody
if not kc.allReady():
print("WARNING: kc.allReady() is failing - call mb.resetData()")
return
mb.createStickParts()
# hide all the stick parts
"""
_toggleVisibleBodies(cstate,
mb.sortedPhysicalBodiesList() + [mb.virtualRoot()],
self.context,
layers=ks.LAYER_STICK_FIGURE,
)
"""
# show this bodies stick parts
_toggleVisibleBodies(
cstate,
[bd],
self.context,
layers=ks.LAYER_STICK_FIGURE,
)
# show/hide stick parts (only webscene)
self._wpbd_stick = kw.Toggle(
router,
text="Stick parts",
on_toggle=lambda cstate: stickCB(cstate, self.item),
tooltip="Toggle visibility of stick parts for the body in 3D graphics",
render_as_button=True,
)
# toggle wire frame view (only webscene)
self._wpbd_wireframe = kw.Button(
router,
text="WireFrame (TBD)",
on_press=lambda: _wireframeBodies([self.item]),
tooltip="Toggle wireframe mode for the 3D parts for the body",
)
# toggle transparent view (only webscene)
self._wpbd_transparent = kw.Button(
router,
text="Semi-transparent (TBD)",
on_press=lambda: _transparentBodies([self.item]),
tooltip="Toggle transparent mode for the 3D parts for the body",
)
self._wpbd_layout_geom_select = widgetArray(
router,
label="Geometry",
children=[
self._wpbd_mesh,
self._wpbd_collision,
self._wpbd_stick,
self._wpbd_wireframe,
self._wpbd_transparent,
],
kind="inputgroup",
)
# ---------------------------------
# for constraints involving the body show line between this body and the other bodies
self._wpbd_constraints = kw.Button(
router,
text="Toggle highlighting",
on_press=lambda: _highlightBodyConstraints(body=self.item, gui_context=self.context),
tooltip="Toggle the highlighting of the constraints and nodes involving the body",
)
# self.wroot.addChild(self._wpbd_constraints)
self._wpbd_layout_geom = widgetArray(
router,
label="Visualize geomatry and structure",
children=[self._wpbd_layout_geom_select, self._wpbd_constraints],
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
self._wpbd_layout_geom.setTooltip(
"Options to use highlighting to visualize system structure"
)
# self.wroot.addChild(self._wpbd_layout_geom)
# ---------------------------------
# ---------------------------------
self._wpbd_bdframe = kw.Toggle(
router,
text="Body",
on_toggle=lambda cstate: _highlightFrames2(
[self.item], cstate, gui_context=self.context, priority_level="primary"
),
render_as_button=True,
tooltip="Toggle the body frame axes",
)
self._wpbd_pnodeframe = kw.Toggle(
router,
text="Pnode",
# on_press=lambda: _highlightFrames(
on_toggle=lambda cstate: _highlightFrames2(
[self.item.pnode()], cstate, gui_context=self.context, priority_level="primary"
),
render_as_button=True,
tooltip="Toggle the body pnode frame axes",
)
self._wpbd_ponodeframe = kw.Toggle(
router,
text="Parent onode",
# on_press=lambda: _highlightFrames(
on_toggle=lambda cstate: _highlightFrames2(
[
self.item.onode()
if (not self.item.isCompoundBody())
else cast(kd.CompoundBody, self.item).physicalParentBody()
],
cstate,
gui_context=self.context,
priority_level="secondary",
),
render_as_button=True,
tooltip="Toggle the frame axes for the parent onode",
)
self._wpbd_conodeframes = kw.Toggle(
router,
text="Child onodes",
# on_press=lambda: _highlightFrames(
on_toggle=lambda cstate: _highlightFrames2(
[
cast(kd.PhysicalBody, x).onode()
for x in self.subtree.childrenBodies(self.item)
if not x.isCompoundBody()
],
cstate,
gui_context=self.context,
priority_level="secondary",
),
render_as_button=True,
tooltip="Toggle the frame axes for all the children body onodes",
)
self._wpbd_subhingeframes = kw.Toggle(
router,
text="Subhinge frames",
# on_press=lambda: _highlightFrames(
on_toggle=lambda cstate: _highlightFrames2(
[self.item.onode()] + [x.pframe() for x in self.item.parentHinge().subhinges()],
cstate,
gui_context=self.context,
priority_level="tertiary",
),
render_as_button=True,
tooltip="Toggle the frame axes for all subhinge oframes and pframes for the body",
)
self._wpbd_nodeframes = kw.Toggle(
router,
text="Nodes",
# on_press=lambda: _highlightFrames(
on_toggle=lambda cstate: _highlightFrames2(
self.item.nodeList(), cstate, gui_context=self.context, priority_level="tertiary"
),
render_as_button=True,
tooltip="Toggle the frame axes for all the regular nodes on the body",
)
self._wpbd_cnodeframes = kw.Toggle(
router,
text="Constraint nodes",
# on_press=lambda: _highlightFrames(
on_toggle=lambda cstate: _highlightFrames2(
self.item.constraintNodeList(),
cstate,
gui_context=self.context,
priority_level="tertiary",
),
render_as_button=True,
tooltip="Toggle the frame axes for all the constraint nodes on the body",
)
# self._wpbd_md_frames = kw.Markdown(router, text="**Frames**", in_line=True)
self._wpbd_layout_frames = widgetArray(
router,
label="View body related frames",
children=[
self._wpbd_bdframe,
self._wpbd_ponodeframe,
self._wpbd_pnodeframe,
self._wpbd_conodeframes,
self._wpbd_subhingeframes,
self._wpbd_nodeframes,
self._wpbd_cnodeframes,
],
kind="accordion", # "inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_frames)
# -------------------------------------
# Record which vizutils visualizations are active
self._libf_cbs = {}
self._aibf_cbs = {}
self._lexf_cbs = {}
self._aexf_cbs = {}
use_ta = False
# scale = 1
radius = 0.01
def _toggleForcesViz(bd, ftype, color, cbsmap, cstate):
id_ = bd.id()
scene = self.context.scene
if not cstate and id_ not in cbsmap:
return
if id_ in cbsmap:
dark = cbsmap.pop(id_)
else:
# print("CCCCC", id_, ftype, bd.name())
if ftype == 0:
dark = vizutils.visualizeInterBodyForce(bd.onode(), scene, use_ta)
elif ftype == 1:
dark = vizutils.visualizeInterBodyTorque(bd.onode(), scene, use_ta)
elif ftype == 2:
dark = vizutils.ScaledVectorVisualizer(
f"{bd}_force_viz",
bd,
scene,
lambda: bd.externalSpatialForce(with_constraints).getv(),
)
elif ftype == 3:
dark = vizutils.ScaledVectorVisualizer(
f"{bd}_torque_viz",
bd,
scene,
lambda: bd.externalSpatialForce(with_constraints).getw(),
)
else:
raise ValueError("Type is unknown")
scale = self._frc_scale
# print("FFFF", scale)
dark.setColor(color)
dark.setScale(scale)
dark.setRadius(radius)
if cstate:
dark.registerCallback()
else:
dark.unregisterCallback()
cbsmap[id_] = dark
scene.update()
# enable interbody force visualization for the body (only webscene)
self._wpbd_interbody_force = kw.Toggle(
router,
text="Interbody force",
on_toggle=lambda cstate: _toggleForcesViz(
self.item, 0, ks.Color.RED, self._libf_cbs, cstate
),
render_as_button=True,
tooltip="Toggle the visualization of the interbody forces for the body",
)
# enable interbody moment visualization for the body (only webscene)
self._wpbd_interbody_moment = kw.Toggle(
router,
text="Interbody moment", # on_toggle=lambda: self._doTbd()
on_toggle=lambda cstate: _toggleForcesViz(
self.item, 1, ks.Color.GREEN, self._aibf_cbs, cstate
),
render_as_button=True,
tooltip="Toggle the visualization of the interbody moments for the body",
)
# enable external force visualization for the body (only webscene)
# enable force vector display for the node
with_constraints = False
self._wpbd_external_force = kw.Toggle(
router,
text="Ext force vector", # on_toggle=self._doTbd)
on_toggle=lambda cstate: _toggleForcesViz(
self.item, 2, ks.Color.BLUE, self._lexf_cbs, cstate
),
render_as_button=True,
tooltip="Toggle the visualization of the net external forces on the body from the force nodes",
)
# enable external moment visualization for the body (only webscene)
self._wpbd_external_moment = kw.Toggle(
router,
text="Ext moment vector",
on_toggle=lambda cstate: _toggleForcesViz(
self.item, 3, ks.Color.YELLOW, self._aexf_cbs, cstate
),
render_as_button=True,
tooltip="Toggle the visualization of the net external moment on the body from the force nodes",
)
# self._wpbd_md_forces = kw.Markdown(router, text="**Forces**", in_line=True)
self._wpbd_layout_forces = widgetArray(
router,
label="Select body force/moment type",
children=[
self._wpbd_interbody_force,
self._wpbd_interbody_moment,
self._wpbd_external_force,
self._wpbd_external_moment,
],
alignment="row",
alignItems="left",
kind="inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_forces)
# ---------------------------------------
slider_opts = kw.SliderOptions()
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Factor to scale up/down all body forces"
self._frc_scale = 0
# def scaleContact(new_scale):
def _scaleForcesViz(new_scale):
# global scale
id_ = self.item.id()
if id_ in self._libf_cbs:
self._libf_cbs[id_].setScale(new_scale)
if id_ in self._aibf_cbs:
self._aibf_cbs[id_].setScale(new_scale)
if id_ in self._lexf_cbs:
self._lexf_cbs[id_].setScale(new_scale)
if id_ in self._aexf_cbs:
self._aexf_cbs[id_].setScale(new_scale)
self._frc_scale = new_scale
# print('EEEE', self._frc_scale)
# self.context.scene.update()
self._wpbd_frc_scale = kw.Slider(
router,
"Viz Force Scale",
_scaleForcesViz,
slider_opts,
)
self._wpbd_frc_scale.setValue(0.05)
# self.wroot.addChild(self._wpbd_frc_scale)
self._wpbd_layout_forces2 = widgetArray(
router,
label="Visualize forces",
children=[self._wpbd_layout_forces, self._wpbd_frc_scale],
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_forces2)
# ---------------------------------
self._wpbd_layout_viz = widgetArray(
router,
label="Body related visualization",
children=[
self._wpbd_layout_geom, # meshes and stick parts
self._wpbd_layout_frames, # body related frames
self._wpbd_layout_forces2,
], # body interaction forces
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wpbd_layout_viz)
# ---------------------------------
def _tocutjointCB():
self.item.toCutJointConstraint()
self.context.multibody.ensureHealthy()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self.context.mbody_tree_view.refresh()
self._wpbd_tocutjoint = kw.Button(
router,
text="To cutjoint",
on_press=lambda: _tocutjointCB(),
tooltip="Convert the hinge into a cut-joint constraint",
)
def _detachCB():
self.item.detach()
self.context.multibody.ensureHealthy()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self.context.mbody_tree_view.refresh()
if len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0:
recreateStickParts(self.context.multibody, self.context)
self._wpbd_detach = kw.Button(
router,
text="Detach body",
on_press=lambda: _detachCB(),
tooltip="Detach the body from its current parent",
)
def _makeBaseCB():
self.item.makeIntoBaseBody()
self.context.multibody.ensureHealthy()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self.context.mbody_tree_view.refresh()
if len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0:
recreateStickParts(self.context.multibody, self.context)
self._wpbd_makebase = kw.Button(
router,
text="Make base body",
on_press=lambda: _makeBaseCB(),
tooltip="Convert this body into a base body",
)
def _discardCB():
self.context.selection.set(kw.Selection().dump())
kc.discard(self.item)
self.context.multibody.ensureHealthy()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self.context.mbody_tree_view.refresh()
if len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0:
recreateStickParts(self.context.multibody, self.context)
self._wpbd_discard = kw.Button(
router,
text="Discard body",
on_press=lambda: _discardCB(),
tooltip="Discard this body",
)
self._wpbd_layout_constraints = widgetArray(
router,
label="Modify body's parent hinge", # self._wpbd_md_constraints,
children=[
# self._wpbd_constraints,
self._wpbd_tocutjoint,
self._wpbd_detach,
self._wpbd_discard,
],
kind="accordion", # "inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_constraints)
# -------------------------
self._node_sps_name = "dummy"
def _setNodeSpNameCB(val):
self._node_sps_name = val
self._wpbd_ndname = kw.StringInput(
router,
"Name",
on_change=lambda val: _setNodeSpNameCB(val),
rapid_submit=True,
)
self._wpbd_ndname.setTooltip("The name for the new node/scene part spec")
self._wpbd_ndname.setSizeClass(kw.SizeClass.WIDE)
self._wpbd_ndname.setValue(self._node_sps_name)
# Add a node
def _addNodeCB():
#
nd = kd.Node.lookupOrCreate(self._node_sps_name, self.item)
nd.setBodyToNodeTransform(km.HomTran())
_createFramesVisJs(use_id_labels=False, gui_context=self.context)
_selectObject(nd, self.context.selection, {"subtree_id": self.subtree.id()})
self._wpbd_addnode = kw.Button(
router,
text="Add node",
on_press=lambda: _addNodeCB(),
tooltip="Add a new node to the body",
)
geom_types = ["Box", "Capsule", "Cone", "Cylinder", "RoundFrustum", "Sphere", "File"]
self.geom_selection = 1
self._sfo_filepath = ""
# callback for the 3D pick mode menu
def _geomSelectionCB(val):
self.geom_selection = val
self._wpbd_geomtype = kw.Dropdown(
router,
"Geometry",
geom_types,
lambda val: _geomSelectionCB(val),
)
self._wpbd_geomtype.setIndex(self.geom_selection)
# Add a node
def _addScenePartSpecCB():
# get geometry type
gtype = geom_types[self.geom_selection]
# create a scene part spec
if gtype != "File":
spec = ks.ScenePartSpec()
if gtype == "Box":
spec.geometry = ks.BoxGeometry(1, 1, 1)
elif gtype == "Capsule":
spec.geometry = ks.CapsuleGeometry(1, 1)
elif gtype == "Cone":
spec.geometry = ks.ConeGeometry(1, 1)
elif gtype == "Cylinder":
spec.geometry = ks.CylinderGeometry(1, 1)
elif gtype == "RoundFrustum":
spec.geometry = ks.RoundFrustumGeometry(1, 1, 1)
elif gtype == "Sphere":
spec.geometry = ks.SphereGeometry(1)
elif gtype == "File":
spec.geometry = ks.SphereGeometry(0.1)
else:
assert 0
spec.material = ks.defaultMaterial()
spec.name = self._node_sps_name
spec.scale = np.ones(3) * 0.1
spec.transform = km.HomTran()
spec.layers = ks.LAYER_PHYSICAL
# add it to the body
self.item.addScenePartSpec(spec)
sp = self.item.getScenePart(spec.name)
_selectObject(sp, self.context.selection, None)
else:
spec = ks.SceneFileObjectSpec()
spec.filepath = Path(self._sfo_filepath)
if not (spec.filepath.is_file() and spec.filepath.exists()):
raise ValueError(f"The {self._sfo_filepath} file does not exist")
spec.name = self._node_sps_name
spec.scale = 1
spec.transform = km.HomTran()
spec.layers = ks.LAYER_PHYSICAL
# spec.filepath = self._sfo_filepath
self.item.addSceneFileObjectSpec(spec)
sfo = self.item.getSceneFileObject(spec.name)
_selectObject(sfo, self.context.selection, None)
self._wpbd_addsps = kw.Button(
router,
text="Add scene part",
on_press=lambda: _addScenePartSpecCB(),
tooltip="Add a new scene part spec to the body",
)
self._sfo_filepath = ""
def _setSfoFilePathCB(val):
self._sfo_filepath = val
self._wpbd_sfofile = kw.StringInput(
router,
"SFO Fliename",
on_change=lambda val: _setSfoFilePathCB(val),
rapid_submit=True,
)
self._wpbd_sfofile.setTooltip("The file path for the new scene file object")
self._wpbd_sfofile.setSizeClass(kw.SizeClass.WIDE)
self._wpbd_sfofile.setValue("")
self._wpbd_layout_nodesps = widgetArray(
router,
label="Add node, scene part", # self._wpbd_md_constraints,
children=[
self._wpbd_addnode,
self._wpbd_ndname,
self._wpbd_addsps,
self._wpbd_geomtype,
self._wpbd_sfofile,
],
kind="accordion", # "inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_nodesps)
# -------------------------
self.hinge_selection: int = 0
hinge_types = ["REVOLUTE", "SLIDER", "BALL", "UJOINT", "GIMBAL", "CYLINDRICAL", "HELICAL"]
hinge_types_enum = [
kd.HingeType.REVOLUTE,
kd.HingeType.SLIDER,
kd.HingeType.BALL,
kd.HingeType.UJOINT,
kd.HingeType.GIMBAL,
kd.HingeType.CYLINDRICAL,
kd.HingeType.HELICAL,
]
def _attachChainCB(cstate):
# print("HH", self.context.visjs_frames_server)
vserver = self.context.visjs_servers[self.context.multibody.id()][0]
# print("KKKK", cstate, vserver.visjs_noselect_cb)
if cstate:
# TODO - Highlight the 6dof bodies available for attachment
def attachAction(id):
# look up body from id
obj = kc.BaseContainer.singleton().at(id)
if not isinstance(obj, kd.PhysicalBody):
raise ValueError(
f"Only a physical body can be attached to another. '{obj.name()}' ({obj.typeString()}) is not a valid choice."
)
obd = cast(kd.PhysicalBody, obj)
assert obj.parentHinge().hingeType() == kd.HingeType.FULL6DOF
if self.item.id() == obj.physicalParentBody().id():
if not self.item.isRootBody():
kc.warn(
f"The '{obj.name()}' is already attached to the '{self.item.name()}' body. Nothing to do!"
)
return
# get the desired hinge type from the dropdown
htype = hinge_types_enum[self.hinge_selection]
# attach the body
print(
f"Attaching '{obd.name()}' to '{self.item.name()}' via '{hinge_types[self.hinge_selection]}' hinge"
)
obd.reattach(self.item, htype)
nhge = obd.parentHinge()
if nhge.coordData().nU() == 1:
cast(kd.Physical1DofSubhinge, nhge.subhinge(0)).setUnitAxis([1, 0, 0])
nhge.coordData().setQ(0)
nhge.coordData().setU(0)
nhge.coordData().setUdot(0)
nhge.coordData().setT(0)
obd.setBodyToJointTransform(km.HomTran())
if not self.item.isRootBody():
obd.onode().setBodyToNodeTransform(km.HomTran())
self.context.multibody.ensureHealthy()
# update stick parts
_createSubTreeVisJs(self.context.multibody, self.context)
# set the noselect callback on the graph server as
# well - TODO: do we need this?
self.context.visjs_servers[self.context.multibody.id()][
0
].visjs_noselect_cb = attachAction
# update highlighting to stay on the currently selected body
_highlightBodies(True, [self.item], [], [], self.context)
if len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0:
recreateStickParts(self.context.multibody, self.context)
# TODO - not sure why this is needed, but need it to
# get the new graph's highlighting updated
# self.context.effects.graph_highlighter._syncGraphs()
# _selectObject(self.item, self.context.selection)
print("==> Pick a 6dof body to attach it to the selected body")
vserver.visjs_noselect_cb = attachAction
# reset the other no select button
# self._wpbd_attachbd.setValue(False)
# self._wpbd_add_cutjoint.setValue(False)
else:
# TODO - Disable the highlighting of the 6dof bodies available for attachment
print("==> Returning to regular body selection mode")
vserver.visjs_noselect_cb = None
self._wpbd_attachbd = kw.Toggle(
router,
text="Attach 6dof body",
on_toggle=lambda cstate: _attachChainCB(cstate),
tooltip="Attach the other 6 dof body to this body via the specified hinge type",
render_as_button=True,
)
# callback for the 3D pick mode menu
def _hingeSelectionCB(val):
self.hinge_selection = val # hinge_types[val]
self._wpbd_hinge_type = kw.Dropdown(
router,
"Hinge type",
hinge_types,
lambda val: _hingeSelectionCB(val),
)
self._wpbd_hinge_type.setIndex(0)
def _cutjointCB(cstate):
# print("HH", self.context.visjs_frames_server)
vserver = self.context.visjs_servers[self.context.multibody.id()][0]
if cstate:
def cutjointAction(id):
# look up body from id
obj = kc.BaseContainer.singleton().at(id)
assert isinstance(obj, kd.PhysicalBody)
if not isinstance(obj, kd.PhysicalBody):
raise ValueError(
f"Can create cut-joint loop constraints only between physical bodies. '{obj.name()}' ({obj.typeString()}) is not a valid choice."
)
obd = cast(kd.PhysicalBody, obj)
# assert obj.parentHinge().hingeType() == kd.HingeType.FULL6DOF
# get the desired hinge type from the dropdown
htype = hinge_types_enum[self.hinge_selection]
# attach the body
prefix = f"{self.item.name()}_{obd.name()}"
print(
f"Creating cut-joint loop constraint between '{obd.name()}' and '{self.item.name()}' with '{hinge_types[self.hinge_selection]}' hinge"
)
if self.item.isRootBody():
cnd1 = kf.Frame.create(prefix + "cfrm1", self.item.container())
f2f = kf.PrescribedFrameToFrame.create(self.item, cnd1)
else:
cnd1 = kd.ConstraintNode.lookupOrCreate(prefix + "cnd1", self.item)
if obd.isRootBody():
cnd2 = kf.Frame.create(prefix + "cfrm2", self.item.container())
f2f = kf.PrescribedFrameToFrame.create(obd, cnd2)
else:
cnd2 = kd.ConstraintNode.lookupOrCreate(prefix + "cnd2", obd)
cj = kd.LoopConstraintCutJoint.create(
prefix + "_cutjt", self.context.multibody, cnd1.frameToFrame(cnd2), htype
)
nhge = cj.hinge()
if nhge.coordData().nU() == 1:
cast(kd.Physical1DofSubhinge, nhge.subhinge(0)).setUnitAxis([1, 0, 0])
nhge.coordData().setQ(0)
nhge.coordData().setU(0)
nhge.coordData().setUdot(0)
nhge.coordData().setT(0)
self.context.multibody.ensureHealthy()
# update stick parts
_createSubTreeVisJs(self.context.multibody, self.context)
# change selection to the new loop constraint
_selectObject(cj, self.context.selection)
print(
"==> Pick a body to create a cut-joint loop constraint between the selected body and the picked body"
)
vserver.visjs_noselect_cb = cutjointAction
# reset the other no select button
self._wpbd_attachbd.setValue(False)
# self._wpbd_add_cutjoint.setValue(False)
else:
print("==> Returning to regular body selection mode")
vserver.visjs_noselect_cb = None
self._wpbd_add_cutjoint = kw.Toggle(
router,
text="Add cut-joint constraint",
on_toggle=lambda cstate: _cutjointCB(cstate),
tooltip="Create a cut-joint loop constraint with the specified hinge type between this and the other body",
render_as_button=True,
)
self._wpbd_layout_attbd = widgetArray(
router,
label="Add hinges and cut-joints (via pick mode)",
children=[self._wpbd_attachbd, self._wpbd_hinge_type, self._wpbd_add_cutjoint],
kind="accordion", # "inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_attbd)
# --------------------------
def _setInb2JointCB(T: km.HomTran):
self.item.onode().setBodyToNodeTransform(T)
self.context.scene.update()
self._wpbd_ib2j = HomTranWidgets(
"Inboard to joint transform", _setInb2JointCB, self.context
)
# self.wroot.addChild(self._wpbd_ib2j._wlayout_homtran)
# --------------------------
def _setBody2JointCB(T: km.HomTran):
self.item.setBodyToJointTransform(T)
self.context.scene.update()
self._wpbd_b2j = HomTranWidgets("Body to joint transform", _setBody2JointCB, self.context)
# self.wroot.addChild(self._wpbd_b2j._wlayout_homtran)
self._wpbd_layout_transforms = widgetArray(
router,
label="Parent hinge location transforms",
children=[self._wpbd_ib2j._wlayout_homtran, self._wpbd_b2j._wlayout_homtran],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wpbd_layout_transforms.setTooltip(
"Modify the parent hinge's location transforms for the body"
)
# self.wroot.addChild(self._wpbd_layout_transforms)
self._wpbd_layout_structure = widgetArray(
router,
label="Modify body and its connectivity",
children=[
self._wpbd_layout_transforms,
self._wpbd_layout_constraints, # modify the parent hinge
self._wpbd_layout_nodesps, # add nodes and scene parts
self._wpbd_layout_attbd, # attach body, create cut-joint
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self.wroot.addChild(self._wpbd_layout_structure)
self._wpbd_layout_structure.setTooltip("Options to modify the body a0nd its connections")
# ---------------------------------
self._sg_branches = []
self._sg_stopats = []
self._sg_name = "dummy"
def _setBranchesCB(cstate):
vserver = self.context.visjs_servers[self.context.multibody.id()][0]
if cstate:
# self._wpbd_sgbranches.setValue(False)
self._wpbd_sgstopats.setValue(False)
self._sg_branches = []
def branchesAction(id):
# look up body from id
obj = kc.BaseContainer.singleton().at(id)
assert isinstance(obj, kd.PhysicalBody)
obd = cast(kd.PhysicalBody, obj)
self._sg_branches.append(obd)
print("==> Pick bodies to select branches to include in the subgraph")
vserver.visjs_noselect_cb = branchesAction
else:
# TODO - Disable the highlighting of the 6dof bodies available for attachment
print("==> Terminating branch selection")
vserver.visjs_noselect_cb = None
self._wpbd_sgbranches = kw.Toggle(
router,
text="Select branches",
on_toggle=lambda cstate: _setBranchesCB(cstate),
tooltip="Select bodies whose branches to include in the subgraph",
render_as_button=True,
)
def _setStopatsCB(cstate):
vserver = self.context.visjs_servers[self.context.multibody.id()][0]
if cstate:
self._wpbd_sgbranches.setValue(False)
# self._wpbd_sgstopats.setValue(False)
self._sg_stopats = []
def stopatsAction(id):
# look up body from id
obj = kc.BaseContainer.singleton().at(id)
assert isinstance(obj, kd.PhysicalBody)
obd = cast(kd.PhysicalBody, obj)
self._sg_stopats.append(obd)
print("==> Pick 'stop at' bodies to limit the bodies included in the subgraph")
vserver.visjs_noselect_cb = stopatsAction
else:
print("==> Terminating stop at selection")
vserver.visjs_noselect_cb = None
self._wpbd_sgstopats = kw.Toggle(
router,
text="Select stopats",
on_toggle=lambda cstate: _setStopatsCB(cstate),
tooltip="Select 'stop at' bodies to limit the bodies to include in the subgraph",
render_as_button=True,
)
def setSGName(val: str):
self._sg_name = val
self._wpbd_sgname = kw.StringInput(
router,
"Name",
on_change=lambda val: setSGName(val),
)
self._wpbd_sgname.setTooltip("The name for the new subgraph")
self._wpbd_sgname.setSizeClass(kw.SizeClass.MEDIUM)
self._wpbd_sgname.setValue(self._sg_name)
# create a subgraph rooted at this body
def _mksubgraphCB():
self._wpbd_sgbranches.setValue(False)
self._wpbd_sgstopats.setValue(False)
print("Root", self.item.name())
print("Branches", [x.name() for x in self._sg_branches])
print("Stop ats", [x.name() for x in self._sg_stopats])
if isinstance(self.subtree, kd.SubGraph):
sg = kd.SubGraph.create(
self._sg_name, self.subtree, self.item, self._sg_branches, self._sg_stopats
)
else:
sg = kd.SubGraph.create(
self._sg_name,
self.subtree.multibody(),
self.item,
self._sg_branches,
self._sg_stopats,
)
_highlightBodies(True, sg.sortedBodiesList(), [self.item], [], self.context)
self.context.subtrees_tree_view.refresh()
_createSubTreeVisJs(sg, self.context)
self._sg_branches = []
self._sg_stopats = []
vserver = self.context.visjs_servers[self.context.multibody.id()][0]
vserver.visjs_noselect_cb = None
self._wpbd_sgbranches.setValue(False)
self._wpbd_sgstopats.setValue(False)
self._wpbd_mksubgraph = kw.Button(
router,
text="Make subgraph",
on_press=lambda: _mksubgraphCB(),
tooltip="Create a subgraph with this body as virtual root",
)
# self._wpbd_md_constraints = kw.Markdown(router, text="**Constraints**", in_line=True)
self._wpbd_layout_sg = widgetArray(
router,
label="Create new SubTree/SubGraph", # self._wpbd_md_constraints,
children=[
# self._wpbd_constraints,
self._wpbd_sgname,
self._wpbd_sgbranches,
self._wpbd_sgstopats,
self._wpbd_mksubgraph,
],
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wpbd_layout_sg)
# ----------------------------------------
self._wpbd_collision_filters = kw.Button(
router,
text="Collsion filters (TBD)",
on_press=lambda: self._doTbd(),
tooltip="Toggle highlighting of bodies whose collisions with this body are being ignored",
)
# self._wpbd_md_collision = kw.Markdown(router, text="**Collision**", in_line=True)
self._wpbd_layout_collision = widgetArray(
router,
label="Collision",
children=[self._wpbd_collision_filters],
kind="inputgroup",
)
# self.wroot.addChild(self._wpbd_layout_collision)
# --------------------------------------------
def teardown(self, _: kd.PhysicalBody, /):
self._wpbd_interbody_force.setValue(False)
self._wpbd_interbody_moment.setValue(False)
self._wpbd_external_force.setValue(False)
self._wpbd_external_moment.setValue(False)
self._wpbd_bdframe.setValue(False)
self._wpbd_ponodeframe.setValue(False)
self._wpbd_pnodeframe.setValue(False)
self._wpbd_conodeframes.setValue(False)
self._wpbd_subhingeframes.setValue(False)
self._wpbd_nodeframes.setValue(False)
self._wpbd_cnodeframes.setValue(False)
self._wpbd_attachbd.setValue(False)
self._wpbd_add_cutjoint.setValue(False)
def getSummary(self) -> str:
item = self.item
result = super().getSummary()
"""
if item.isRootBody():
return "Root body"
else:
hge_type = kd.HingeBase.hingeTypeString(item.parentHinge().hingeType())
result = super().getSummary()
return result # + f"Hinge: {hge_type}"
"""
return result
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
return isinstance(item, kd.PhysicalBody)
def setup(self, item: kd.PhysicalBody, item_context: kw.Json, /):
not_root_body = not item.isRootBody()
# set the context subtree
if isinstance(item_context, dict):
# set the context subtree
st_id = item_context.get("subtree_id", None)
if not isinstance(st_id, int):
raise ValueError("Could not get the subtree ID.")
self._subtree = cast(kd.SubTree, kc.BaseContainer.singleton().at(st_id))
if not isinstance(self._subtree, kd.SubTree):
raise ValueError(f"Did not get a SubTree from ID {st_id}.")
else:
if not item.isCompoundBody():
kc.warn(
f"The subtree value is missing when selecting the {item.name()} body. Defaulting to multibody."
)
self._subtree = self.context.multibody
else:
kc.warn(
f"The subtree value is missing when setting up the {item.name()} compound body."
)
super().setup(item, item_context)
if not_root_body:
self.init_Q = item.parentHinge().coordData().getQ()
self.full_Q = self.context.multibody.getQ()
is_not_locked = not_root_body and item.parentHinge().hingeType() != kd.HingeType.LOCKED
has_constraint_nodes = len(item.constraintNodeList()) > 0
has_scene_parts = len(item.getSceneParts()) > 0
has_stick_parts = len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0
has_body_loop_constraints = len(self.context.multibody.getBodyLoopConstraints(item)) > 0
is_base_body = self.context.multibody.isBaseBody(item)
is_floating_body = not_root_body and item.parentHinge().hingeType() == kd.HingeType.FULL6DOF
parent_is_floating_body = (
not_root_body
and (not item.physicalParentBody().isRootBody())
and item.physicalParentBody().parentHinge().hingeType() == kd.HingeType.FULL6DOF
)
floating_bodies = [
x
for x in self.context.multibody.sortedPhysicalBodiesList()
if x.parentHinge().hingeType() == kd.HingeType.FULL6DOF
]
has_6dof_bodies = len(floating_bodies) > 0
self._wpbd_ib2j._wlayout_homtran.setVisible(not_root_body and not is_base_body)
self._wpbd_b2j._wlayout_homtran.setVisible(not_root_body and not is_floating_body)
if not_root_body:
self._wpbd_ib2j.setup(item.onode().getBodyToNodeTransform())
self._wpbd_b2j.setup(item.getBodyToJointTransform())
self._wpbd_attachbd.setEnabled(has_6dof_bodies)
if not_root_body:
self._frame_pair_widgets.setup(
kc.CppWeakRef(item.parentHinge()), kc.CppWeakRef(self.subtree)
)
else:
assert self.subtree
self._frame_pair_widgets.setup(None, kc.CppWeakRef(self.subtree))
self._wpbd_frc_scale.setVisible(not_root_body)
self._wpbd_subhingeframes.setVisible(is_not_locked)
self._wpbd_constraints.setVisible(has_body_loop_constraints)
self._wpbd_tocutjoint.setVisible(not is_floating_body)
self._wpbd_detach.setVisible(not is_floating_body)
self._wpbd_makebase.setVisible(parent_is_floating_body)
self._wpbd_layout_geom.setVisible(not_root_body)
self._wpbd_mesh.setVisible(not_root_body)
self._wpbd_mesh.setValue(True)
self._wpbd_stick.setVisible(not_root_body)
self._wpbd_stick.setValue(has_stick_parts)
# self._wpbd_scale_stick.setVisible(not_root_body)
# self._wpbd_md_collision.setVisible(not_root_body)
self._wpbd_collision_filters.setVisible(not_root_body)
self._wpbd_collision.setValue(False)
# until implemented
self._wpbd_wireframe.setVisible(not_root_body)
self._wpbd_transparent.setVisible(not_root_body)
# self._wpbd_md_forces.setVisible(not_root_body)
self._wpbd_interbody_force.setVisible(not_root_body)
self._wpbd_interbody_moment.setVisible(not_root_body)
self._wpbd_external_force.setVisible(not_root_body)
self._wpbd_external_moment.setVisible(not_root_body)
self._wpbd_pnodeframe.setVisible(not_root_body)
self._wpbd_subhingeframes.setVisible(not_root_body)
self._wpbd_nodeframes.setVisible(not_root_body)
self._wpbd_cnodeframes.setVisible(has_constraint_nodes)
self._wpbd_layout_geom.setVisible(has_scene_parts)
self._wpbd_mesh.setVisible(has_scene_parts)
# TODO - add implementation
if 1:
# self._wpbd_md_collision.setVisible(False)
self._wpbd_collision_filters.setVisible(False)
# until implemented
self._wpbd_wireframe.setVisible(False)
self._wpbd_transparent.setVisible(False)
# Assigning the new item explicitly and early because below we
# are setting states that may trigger callbacks which expect
# self.item to be current.
# self.item = item
self._wpbd_bdframe.setValue(False)
self._wpbd_pnodeframe.setValue(False)
self._wpbd_conodeframes.setValue(False)
self._wpbd_subhingeframes.setValue(False)
self._wpbd_nodeframes.setValue(False)
self._wpbd_cnodeframes.setValue(False)
self._wpbd_interbody_force.setValue(False)
self._wpbd_interbody_moment.setValue(False)
self._wpbd_external_force.setValue(False)
self._wpbd_external_moment.setValue(False)
@property
def subtree(self):
# AG - needs to be updated to return true subgraph context
# return self.context.multibody
if not hasattr(self, "_subtree"):
raise ValueError("Subtree has not been set yet.")
return self._subtree
def close(self):
if hasattr(self, "_subtree"):
del self._subtree
for v in self._libf_cbs.values():
v()
self._libf_cbs.clear()
for v in self._aibf_cbs.values():
v()
self._aibf_cbs.clear()
for v in self._lexf_cbs.values():
v()
self._lexf_cbs.clear()
for v in self._aexf_cbs.values():
v()
self._aexf_cbs.clear()
@register
class CompoundBodyCard(BodyBaseCard): # AbstractCard[kd.CompoundBody]):
"""Card to display info about a CompoundBody."""
@property
def label(self) -> str:
return "CompoundBody"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wcbd_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wcbd_header)
self._wcbd_header1 = kw.Markdown(router, "**CompoundBody**")
self.wroot.addChild(self._wcbd_header1)
# ---------------------------
self._cmphge_widgets = CompoundHingeWidgets(self.context)
self.wroot.addChild(self._cmphge_widgets._wik_layout)
self._cmphge_widgets._wik_layout.setOpen(True)
# ----------------------------
# open tab with visjs display of the bodies in the compound body
self._wcbd_visjs_bodies = kw.Button(
router,
text="Bodies",
on_press=lambda: _createSubTreeVisJs(self.item.bodiesTree(), self.context),
tooltip="Create, if needed, the visjs graph of the bodies in this subtree",
)
# open tab with visjs display of the physical bodies in the compound body
self._wcbd_visjs_physical = kw.Button(
router,
text="Physical bodies",
on_press=lambda: _createSubTreeVisJs(self.item.physicalBodiesTree(), self.context),
tooltip="Create, if needed, the visjs graph of the physical bodies in this subtree",
)
# self._wcbd_md_bodies = kw.Markdown(router, text="**Visjs graph**", in_line=True)
self._wcbd_layout_visjs = widgetArray(
router,
label="Create Visjs Graph",
children=[self._wcbd_visjs_bodies, self._wcbd_visjs_physical],
# kind="inputgroup",
)
# self.wroot.addChild(self._wcbd_layout_embedded)
# ----------------------------
# create treeview for the subtree
self._wcbd_treeview = kw.Button(
router,
text="Bodies",
on_press=lambda: _createSubTreeView(self.item.bodiesTree(), self.context),
tooltip="Create and add TreeView for the compound body's subtree",
)
# create treeview for the subtree
self._wcbd_treeview_physical = kw.Button(
router,
text="Physical bodies",
on_press=lambda: _createSubTreeView(self.item.physicalBodiesTree(), self.context),
tooltip="Create and add TreeView for the the compound body's phhysical bodies subtree",
)
# Setup widget topology
# self._wcbd_md_treeview = kw.Markdown(router, text="**Create TreeView**", in_line=True)
self._wcbd_layout_treeview = widgetArray(
router,
label="Create TreeView",
children=[self._wcbd_treeview, self._wcbd_treeview_physical],
# kind="accordion", # "inputgroup",
)
self._wcbd_layout_treeview.setTooltip(
"Create TreeViews for the bodes embedded within this compound body"
)
self._wcbd_layout_views = widgetArray(
router,
label="Create views",
children=[self._wcbd_layout_treeview, self._wcbd_layout_visjs],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wcbd_layout_treeview.setTooltip(
"Create TreeViews and bodies graphs for the bodies embedded within this compound body"
)
self.wroot.addChild(self._wcbd_layout_views)
# ------------------------------------
def _flattenCB(sg):
if not sg.hasCompoundBodies():
return
sg.flattenCompoundBodies()
# highlight bodies in aggregation graph for a constraint
self._wcbd_flatten = kw.Button(
router,
text="Flatten compound body",
on_press=lambda: _flattenCB(self.item),
tooltip="Flatten the compound body to remove all nested compound bodies and replace with their physical bodies",
)
# self._wcbd_md_flatten = kw.Markdown(router, text="**Flatten**", in_line=True)
self._wcbd_layout_flatten = widgetArray(
router,
label="Transform the compound body",
children=[self._wcbd_flatten],
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wcbd_layout_flatten)
def getSummary(self) -> str:
item = self.item
hge_type = kd.HingeBase.hingeTypeString(item.parentHinge().hingeType())
result = super().getSummary()
return result # + f"Hinge: {hge_type}"
def setup(self, item: kd.CompoundBody, item_context: kw.Json, /):
super().setup(item, item_context)
# hide the physical subtree buttons if the physical bodies are
# the same as the regular bodies
has_compound = item.bodiesTree().hasCompoundBodies()
self._wcbd_visjs_physical.setEnabled(has_compound)
self._wcbd_treeview_physical.setEnabled(has_compound)
self._wcbd_layout_flatten.setEnabled(has_compound)
self._cmphge_widgets.setup(kc.CppWeakRef(item.parentHinge()))
"""
# highlight the embedded physical bodies in the multibody visjs graph
_highlightBodies(
bodies=item.physicalBodiesTree().sortedPhysicalBodiesList(),
secondary_bodies=[],
tertiary_bodies=[],
toggle_over_set=False,
gui_context=self.context,
)
"""
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
return isinstance(item, kd.CompoundBody)
@register
class ProxyScenePartCard(AbstractCard[ks.ProxyScenePart]):
"""Card to display info about a ScenePart."""
@property
def label(self) -> str:
return "ProxyScenePart"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# GRAPHICAL PART
# ---------------------------
# highlight parent node/part, children node/part (different highlighting) (TBD)
self._wdownstream = kw.Button(
router,
text="Downstream (TBD)",
on_press=self._doTbd,
tooltip="Toggle highlighting of the parent and children scene parts",
)
# highlight upstream scene parts
self._wupstream = kw.Button(
router,
text="Upstream (TBD)",
on_press=self._doTbd,
tooltip="Toggle highlighting of the upstream scene parts",
)
# toggle wire frame view (only webscene)
self._wwireframe = kw.Button(
router,
text="WireFrame (TBD)",
on_press=lambda: _wireframe(self.item),
tooltip="Toggle wireframe mode for the scene part",
)
# toggle transparent view (only webscene)
self._wtransparent = kw.Button(
router,
text="Semi-transparent (TBD)",
on_press=lambda: _transparent(self.item),
tooltip="Toggle transparent mode for the scene part",
)
# change color
self._wcolor = kw.Button(
router,
text="Color (TBD)",
on_press=self._doTbd,
tooltip="Change the color for the scene part",
)
# SCENE NODE
# Toggle scene part visibility (only webscene)
self._wvisible = kw.Toggle(
router,
text="Show/hide part",
on_toggle=lambda cstate: self.item.setVisible(cstate),
tooltip="Toggle visibility of the scene part",
render_as_button=True,
)
def _getParentFrameList():
if isinstance(self.item, ks.ProxyScenePart):
return [self.item.ancestorFrame()]
else:
return []
# show frame axes
self._wframe = kw.Toggle(
router,
text="Parent frame",
on_toggle=lambda cstate: _highlightFrames(
cstate, _getParentFrameList(), gui_context=self.context
),
tooltip="Enable the axes for the frame to which this scene part is attached",
render_as_button=True,
)
self._wlayout_highlight = widgetArray(
router,
label="Highlight",
children=[
self._wvisible,
self._wframe,
self._wupstream,
self._wdownstream,
self._wwireframe,
self._wtransparent,
],
kind="inputgroup",
)
self.wroot.addChild(self._wlayout_highlight)
# ---------------------------
# discard part
def _discardCB():
frm = self.item.ancestorFrame()
if not isinstance(frm, kd.PhysicalBody):
raise ValueError(
f"Only scene parts attached to physical bodies are removable this way. The parent frame '{frm.name()} ({frm.typeString()}) is not one."
)
bd = cast(kd.PhysicalBody, frm)
nm = self.item.name()
self.context.selection.set(kw.Selection().dump())
bd.removeScenePartSpec(nm)
self._wdiscard = kw.Button(
router,
text="Discard part",
on_press=lambda: _discardCB(),
tooltip="Discard the scene part",
)
self.wroot.addChild(self._wdiscard)
# --------------------------
if 1:
def _setSimTransformCB(T: km.SimTran, scale):
self.item.setSimTransform(T)
self.item.setIntrinsicScale(scale)
self.context.scene.update()
self._wpart_trans = SimTranWidgets(
"Transform and scaling", _setSimTransformCB, self.context
)
self.wroot.addChild(self._wpart_trans._wlayout_simtran)
def setup(self, item: ks.ProxyScenePart, item_context: kw.Json, /):
super().setup(item, item_context)
# self._wpart_trans._wlayout_scale.setVisible(False)
self._wvisible.setValue(item.getVisible())
if 1:
self._wpart_trans.setup(item.getSimTransform(), item.getIntrinsicScale())
if 1:
# TODO - until implemented
self._wupstream.setVisible(False)
self._wdownstream.setVisible(False)
self._wwireframe.setVisible(False)
self._wtransparent.setVisible(False)
# self._wvisible.setValue(True, trigger_own_callback=True)
# self._wvisible.setValue(True)
@register
class ProxySceneFileObjectCard(AbstractCard[ks.ProxySceneFileObject]):
"""Card to display info about a SceneFileObject."""
@property
def label(self) -> str:
return "ProxySceneFileObject"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# GRAPHICAL PART
# ---------------------------
# SCENE NODE
# Toggle scene part visibility (only webscene)
self._wvisible = kw.Toggle(
router,
text="Show/hide part",
on_toggle=lambda cstate: self.item.setVisible(cstate),
tooltip="Toggle visibility of the scene file object",
render_as_button=True,
)
def _getParentFrameList():
if isinstance(self.item, ks.ProxySceneFileObject):
return [self.item.ancestorFrame()]
else:
return []
# show frame axes
self._wframe = kw.Toggle(
router,
text="Parent frame",
on_toggle=lambda cstate: _highlightFrames(
cstate, _getParentFrameList(), gui_context=self.context
),
tooltip="Enable the axes for the frame to which this scene file object is attached",
render_as_button=True,
)
self._wlayout_highlight = widgetArray(
router,
label="Highlight",
children=[
self._wvisible,
self._wframe,
],
kind="inputgroup",
)
self.wroot.addChild(self._wlayout_highlight)
# ---------------------------
# discard part
def _discardCB():
frm = self.item.ancestorFrame()
if not isinstance(frm, kd.PhysicalBody):
raise ValueError(
f"Only scene file objects attached to physical bodies are removable this way. The parent frame '{frm.name()} ({frm.typeString()}) is not one."
)
bd = cast(kd.PhysicalBody, frm)
nm = self.item.name()
self.context.selection.set(kw.Selection().dump())
bd.removeSceneFileObjectSpec(nm)
self._wdiscard = kw.Button(
router,
text="Discard SFO",
on_press=lambda: _discardCB(),
tooltip="Discard the scene file object",
)
self.wroot.addChild(self._wdiscard)
# --------------------------
if 1:
def _setSimTransformCB(T: km.SimTran, scale):
self.item.setSimTransform(T)
self.context.scene.update()
self._wpart_trans = SimTranWidgets(
"Transform and scaling", _setSimTransformCB, self.context
)
self.wroot.addChild(self._wpart_trans._wlayout_simtran)
def setup(self, item: ks.ProxySceneFileObject, item_context: kw.Json, /):
super().setup(item, item_context)
self._wpart_trans._wlayout_scale.setVisible(False)
self._wvisible.setValue(item.getVisible())
@register
class NodeCard(AbstractCard[kd.Node]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "Node"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
# self._wnd_header = kw.Markdown(router, "---")
# self.wroot.addChild(self._wnd_header)
self._wnd_header1 = kw.Markdown(router, "**Node**")
self.wroot.addChild(self._wnd_header1)
# -----------------
# highlight parent body
self._wnd_body = kw.Toggle(
router,
text="Highlight parent body",
on_toggle=lambda cstate: _highlightBodies(
cstate, [self.item.parentBody()], [], [], gui_context=self.context
),
tooltip="Toggle the highlighting of the node's parent body",
render_as_button=True,
)
# ------------------------
self._lf_cbs = {}
self._af_cbs = {}
scale = 1
radius = 0.01
def _toggleForcesViz(nd, ftype, color, cbsmap):
id_ = nd.id()
scene = self.context.scene
if id_ in cbsmap:
cbsmap.pop(id_)()
# print("Creating")
if ftype == 0:
dark = vizutils.visualizeNodeForce(nd, self.context.scene)
elif ftype == 1:
dark = vizutils.visualizeNodeTorque(nd, self.context.scene)
else:
raise ValueError("Type not recognized")
dark.setScale(scale)
dark.setRadius(radius)
dark.setColor(color)
dark.registerCallback()
cbsmap[id_] = dark
scene.update()
# enable force vector display for the node
self._wnd_force_vec = kw.Button(
router,
text="Force vector", # on_press=self._doTbd)
on_press=lambda: _toggleForcesViz(
self.item,
0,
ks.Color.RED,
self._lf_cbs,
),
tooltip="Toggle the visualization of the external force on the node",
)
# enable moment vector display for the node
self._wnd_moment_vec = kw.Button(
router,
text="Moment vector",
on_press=lambda: _toggleForcesViz(
self.item,
1,
ks.Color.GREEN,
self._af_cbs,
),
tooltip="Toggle the visualization of the external moment on the node",
)
# self._wnd_md_forces = kw.Markdown(router, text="Forces**", in_line=True)
self._wnd_layout_forces = widgetArray(
router,
label="**Node's external spatial force**",
children=[self._wnd_force_vec, self._wnd_moment_vec],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wnd_layout_forces)
self._wnd_layout_viz = widgetArray(
router,
label="Introspection",
children=[self._wnd_body, self._wnd_layout_forces],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wnd_layout_viz.setTooltip(
"Highlight the parent body, and visualize forces at the node"
)
self._wnd_layout_viz.setOpen(True)
self.wroot.addChild(self._wnd_layout_viz)
# --------------------------
def _setBodyToNodeCB(T: km.HomTran):
# T.dump('TT')
self.item.setBodyToNodeTransform(T)
self.context.scene.update()
self._wnd_bd2node = HomTranWidgets("Body to node transform", _setBodyToNodeCB, self.context)
self.wroot.addChild(self._wnd_bd2node._wlayout_homtran)
# ---------------------------
# discard part
def _discardCB():
bd = self.item.parentBody()
_selectObject(bd, self.context.selection, None)
kc.discard(self.item)
_createFramesVisJs(False, self.context)
self._wnd_discard = kw.Button(
router,
text="Discard",
on_press=lambda: _discardCB(),
tooltip="Discard the node",
)
# self.wroot.addChild(self._wnd_detach)
self._wnd_layout_discard = widgetArray(
router,
label="Discard the node",
children=[
# self._wnd_body,
self._wnd_discard,
], # self._wnd_constraint]
kind="accordion", # "inputgroup",
)
self._wnd_layout_discard.setTooltip("Discard this node (if not in use elsewhere)")
self.wroot.addChild(self._wnd_layout_discard)
# --------------------------
# change selection to the parent body
self._wnd_select_up = kw.Button(
router,
text="Up",
on_press=lambda: _selectObject(self.item.parentBody(), self.context.selection),
tooltip="Change the selection to the node's parent body",
)
# change selection to the next sibling node
self._wnd_select_right = kw.Button(
router,
text="Right",
on_press=lambda: _selectObject(_siblingNode(self.item, True), self.context.selection),
tooltip="Change the selection to the next node on the body",
)
# change selection to the previous sibling node
self._wnd_select_left = kw.Button(
router,
text="Left",
on_press=lambda: _selectObject(_siblingNode(self.item, False), self.context.selection),
tooltip="Change the selection to the previous node on the body",
)
"""
# show constraint attached to constraint node
self._wnd_constraint = kw.Button(router, text="Loop constraint (TBD)",
on_press=self._doTbd),
tooltip="Toggle the highlighting of the constraint attached to this constraint node"
"""
# Setup widget topology
# self._wnd_md_highlight = kw.Markdown(router, text="**Highlight**", in_line=True)
# self._wnd_md_selection = kw.Markdown(router, text="**Selection**", in_line=True)
self._wnd_layout_select = widgetArray(
router,
label="Change selection to related node",
children=[self._wnd_select_up, self._wnd_select_left, self._wnd_select_right],
kind="accordion", # "inputgroup",
)
self._wnd_layout_select.setTooltip("Change the selection to a sibling node or parent body")
self.wroot.addChild(self._wnd_layout_select)
def getSummary(self) -> str:
body = self.item.parentBody()
return f"Parent body: {body.name()}"
def setup(self, item: kd.Node, item_context: kw.Json, /):
super().setup(item, item_context)
# hide the header is this is the most derived class
if not isinstance(item, kd.ConstraintNode):
self._wnd_header1.setVisible(False)
is_force_node = item.isExternalForceNode()
self._wnd_layout_forces.setVisible(is_force_node)
self._wnd_bd2node.setup(item.getBodyToNodeTransform())
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
if isinstance(item, kd.ConstraintNode):
return False
return isinstance(item, self.wrapped())
def close(self):
for v in self._lf_cbs.values():
v()
self._lf_cbs.clear()
for v in self._af_cbs.values():
v()
self._af_cbs.clear()
@register
class ConstraintNodeCard(NodeCard): # AbstractCard[kd.ConstraintNode]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "ConstraintNode"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wcnd_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wcnd_header)
self._wcnd_header1 = kw.Markdown(router, "**ConstraintNode**")
self.wroot.addChild(self._wcnd_header1)
# ------------------------------
# for constraints for this node show line between this node and the other bodies
self._wcnd_constraint = kw.Button(
router,
text="Constraints",
on_press=lambda: _showLoopConstraints([self.item.loopConstraint()], self.context),
tooltip="Toggle the highlighting of the bilaateral constraint attached to this constraint node",
)
# self._wcnd_md_constraint = kw.Markdown(router, text="**Constraint**", in_line=True)
self._wcnd_layout_constraint = widgetArray(
router,
label="Constraint",
children=[self._wcnd_constraint],
kind="inputgroup",
)
# ------------------------------
# change selection to the previous sibling node
self._wcnd_select_constraint = kw.Button(
router,
text="Go to loop constraint",
on_press=lambda: _selectObject(self.item.loopConstraint(), self.context.selection),
tooltip="Change the selection to the attached loop constraint",
)
"""
# show constraint attached to constraint node
self._wcnd_constraint = kw.Button(router, text="Loop constraint (TBD)",
on_press=self._doTbd),
tooltip="Toggle the highlighting of the constraint attached to this constraint node"
"""
# Setup widget topology
# self._wcnd_md_highlight = kw.Markdown(router, text="**Highlight**", in_line=True)
# self._wcnd_md_selection = kw.Markdown(router, text="**Selection**", in_line=True)
self._wcnd_layout_select = widgetArray(
router,
label="Go to the loop constraint",
children=[self._wcnd_select_constraint],
kind="accordion", # "inputgroup",
)
self._wcnd_layout_select.setTooltip("Change the selection to attached loop constraint")
self._wcnd_layout_select.setOpen(True)
self.wroot.addChild(self._wcnd_layout_select)
self.wroot.addChild(self._wcnd_layout_constraint)
def getSummary(self) -> str:
item = self.item
lines = []
result = super().getSummary()
lines.append(f"Body: {item.parentBody().name()}")
if lc := item.loopConstraint():
lines.append(f"Loop constraint: {lc.name()}")
else:
lines.append("No loop constraint")
return result + " \n".join(lines)
def setup(self, item: kd.ConstraintNode, item_context: kw.Json, /):
super().setup(item, item_context)
has_loop_constraint = item.loopConstraint() is not None
# not implemented yet
self._wcnd_layout_constraint.setVisible(False)
self._wcnd_layout_select.setEnabled(has_loop_constraint)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
return isinstance(item, kd.ConstraintNode)
@register
class BilateralConstraintBaseCard(AbstractCard[kd.BilateralConstraintBase]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "BilateralConstraintBase"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
# self._wbcb_header = kw.Markdown(router, "---")
# self.wroot.addChild(self._wbcb_header)
self._wbcb_header1 = kw.Markdown(router, "**BilateralConstraintBase**")
self.wroot.addChild(self._wbcb_header1)
"""
def _toggleConstraint(cn):
mb = self.context.multibody
if mb.getEnabledConstraint(cn.name()):
# print("Disabling", cn.name())
mb.disableConstraint(cn)
else:
# print("Enabling", cn.name())
mb.enableConstraint(cn)
# enable/disable the constraint
self._wbcb_enable = kw.Button(
router,
text="Toggle",
on_press=lambda: _toggleConstraint(self.item),
tooltip="Toggle enabling/disabling the constraint",
)
# self._wbcb_md_enable = kw.Markdown(router, text="**Enable/Disable constraint**", in_line=True)
self._wbcb_layout_enable = widgetArray(
router,
label="Enable/disable constraint",
children=[self._wenable],
kind="inputgroup",
)
self.wroot.addChild(self._wlayout_enable)
"""
# ------------------------------------
def _createAggSubgraph(constraint: kd.BilateralConstraintBase):
# first check that a cegraph exists, else create it
mb = self.context.multibody
nbodies = len(mb.sortedPhysicalBodiesList())
cegraph = None
for st in mb.childrenSubTrees():
if (
isinstance(st, kd.SubGraph)
and st.getEnabledConstraint(constraint.name())
and len(st.sortedPhysicalBodiesList()) == nbodies
):
cegraph = st
break
if not cegraph:
cegraph = kd.SubGraph.create(f"cegraph{constraint.id()}", mb, mb.virtualRoot())
# now create the agg subgraph
st = cegraph.aggregationSubGraph(constraint.name() + "agg_sg", [constraint], True)
_highlightBodies(True, st.sortedPhysicalBodiesList(), [], [], self.context)
self.context.subtrees_tree_view.refresh()
# highlight bodies in aggregation graph for a constraint
self._wbcb_aggsg = kw.Button(
router,
text="Create aggregation subgraph",
on_press=lambda: _createAggSubgraph(self.item),
tooltip="Create an aggregation subgraph for this constraint",
)
def _disableCB(cstate):
if cstate:
cast(kd.SubGraph, self._subtree).enableConstraint(self.item)
else:
cast(kd.SubGraph, self._subtree).disableConstraint(self.item)
_createSubTreeVisJs(self._subtree, self.context)
self._wbcb_disable = kw.Toggle(
router,
text="Disable",
on_toggle=lambda cstate: _disableCB(cstate),
tooltip="Disable this constraint",
render_as_button=True,
)
self._wbcb_layout_sg = widgetArray(
router,
label="Sub-graph related changes",
children=[
self._wbcb_aggsg,
self._wbcb_disable,
],
kind="accordion", # "inputgroup",
)
self._wbcb_layout_sg.setTooltip("Make subgraph related changes based on the constraint")
self._wbcb_layout_sg.setOpen(True)
self.wroot.addChild(self._wbcb_layout_sg)
# ------------------
def _discardCB():
self.context.selection.set(kw.Selection().dump())
kc.discard(self.item)
self.context.multibody.ensureHealthy()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self._wbcb_discard = kw.Button(
router,
text="Discard",
on_press=lambda: _discardCB(),
tooltip="Discard this constraint",
)
self._wbcb_layout_constraints = widgetArray(
router,
label="Discard the constraint", # self._wbcb_md_constraints,
children=[
# self._wbcb_constraints,
# self._wbcb_aggsg,
# self._wbcb_disable,
self._wbcb_discard,
],
kind="accordion", # "inputgroup",
)
self._wbcb_layout_constraints.setTooltip("Discard the constraint (cannot undo)")
# self._wbcb_layout_constraints.setOpen(True)
self.wroot.addChild(self._wbcb_layout_constraints)
"""
# for constraints for this node show line between this node and the other bodies
self._wbcb_constraint = kw.Button(
router,
text="Constraints",
on_press=lambda: _showLoopConstraints([self.item.loopConstraint()], self.context),
)
self._wbcb_md_constraint = kw.Markdown(router, text="**Constraint**", in_line=True)
self._wbcb_layout_constraint = widgetArray(
router, label=self._wbcb_md_constraint, children=[self._wbcb_constraint]
)
self.wroot.addChild(self._wbcb_layout_constraint)
"""
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
# this is a non-concrete class
return False
"""
if isinstance(item, kd.LoopConstraintBase):
return False
return isinstance(item, self.wrapped())
"""
def setup(self, item: kd.BilateralConstraintBase, item_context: kw.Json, /):
# set the context subtree
if isinstance(item_context, dict):
# set the context subtree
st_id = item_context.get("subtree_id", None)
if not isinstance(st_id, int):
raise ValueError("Could not get the subtree ID.")
self._subtree = cast(kd.SubTree, kc.BaseContainer.singleton().at(st_id))
if not isinstance(self._subtree, kd.SubTree):
raise ValueError(f"Did not get a SubTree from ID {st_id}.")
else:
kc.warn(
f"The subtree value is missing when selecting the {item.name()} constraint. Defaulting to multibody."
)
self._subtree = self.context.multibody
super().setup(item, item_context)
# highlight the constraint nodes and bodies involved
self._wbcb_disable.setValue(self._subtree.isEnabledConstraint(item))
@register
class LoopConstraintBaseCard(BilateralConstraintBaseCard): # AbstractCard[kd.LoopConstraintBase]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "LoopConstraintBase"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wlcb_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wlcb_header)
self._wlcb_header1 = kw.Markdown(router, "**LoopConstraintBase**")
self.wroot.addChild(self._wlcb_header1)
# ------------------------------
self._wlcb_srcframe = kw.Toggle(
router,
text="Source node",
on_toggle=lambda cstate: _highlightFrames2(
[self.item.constraintFrameToFrame().oframe()],
cstate,
gui_context=self.context,
priority_level="primary",
),
render_as_button=True,
tooltip="Show the source node frame axes",
)
self._wlcb_tgtframe = kw.Toggle(
router,
text="Target node",
on_toggle=lambda cstate: _highlightFrames2(
[self.item.constraintFrameToFrame().pframe()],
cstate,
gui_context=self.context,
priority_level="secondary",
),
render_as_button=True,
tooltip="Show the target node frame axes",
)
# should be in cutjoint card - TODO
self._wlcb_errframe = kw.Toggle(
router,
text="Error frame",
on_toggle=lambda cstate: _highlightFrames2(
[self.item.hinge().pframe()],
cstate,
gui_context=self.context,
priority_level="tertiary",
),
render_as_button=True,
tooltip="Show the error frame axes",
)
# self._wlcb_md_frames = kw.Markdown(router, text="**Constraint nodes**", in_line=True)
self._wlcb_layout_frames = widgetArray(
router,
label="Highlight loop constraint's constraint nodes and error frame",
children=[
self._wlcb_srcframe,
self._wlcb_tgtframe,
self._wlcb_errframe,
],
kind="accordion", # "inputgroup",
)
self._wlcb_layout_frames.setTooltip(
"Turn on highlighting of the constraint frames for this loop constraint"
)
self._wlcb_layout_frames.setOpen(True)
self.wroot.addChild(self._wlcb_layout_frames)
# -----------------------------------
# change selection to the pframe
self._wlcb_select_pframe = kw.Button(
router,
text="Go to target frame",
on_press=lambda: _selectObject(
self.item.constraintFrameToFrame().pframe(), self.context.selection
),
tooltip="Change selection to the target frame for this loop constraint",
)
# change selection to the oframe
self._wlcb_select_oframe = kw.Button(
router,
text="Go to source frame",
on_press=lambda: _selectObject(self.item.oframe(), self.context.selection),
tooltip="Change selection to the source frame for this loop constraint",
)
# self._wlcb_md_selection = kw.Markdown(router, text="**Selection**", in_line=True)
self._wlcb_layout_select = widgetArray(
router,
label="Switch to the constraint frames",
children=[
self._wlcb_select_oframe,
self._wlcb_select_pframe,
],
kind="accordion", # "inputgroup",
)
self._wlcb_layout_select.setTooltip(
"Switch selection to the constraint frames for this loop constraint"
)
self.wroot.addChild(self._wlcb_layout_select)
"""
def getSummary(self) -> str:
item = self.item
if isinstance(item, kd.LoopConstraintCutJoint):
hge_type = kd.HingeBase.hingeTypeString(item.hinge().hingeType())
return f"Hinge: {hge_type}"
else:
return "Not a cut joint"
"""
def setup(self, item: kd.LoopConstraintBase, item_context: kw.Json, /):
super().setup(item, item_context)
is_cutjoint = isinstance(item, kd.LoopConstraintCutJoint)
self._wlcb_errframe.setVisible(is_cutjoint)
def teardown(self, _: kd.LoopConstraintBase, /):
self._wlcb_srcframe.setValue(False)
self._wlcb_tgtframe.setValue(False)
self._wlcb_errframe.setValue(False)
@register
class PhysicalSubhingeCard(AbstractCard[kd.PhysicalSubhinge]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "PhysicalSubhinge"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ---------------------------
# create sliders for max of 6 subhinges, with each having max 3
# coords. we only make visible the ones that are applicable for
# a body
# slider to scale the stick parts
# markdown to show status from IK
self._wmd_coord_ik = kw.Markdown(router, text="**IK status:**")
slider_opts = kw.SliderOptions()
slider_opts.min = -3.2
slider_opts.max = 3.2
slider_opts.step = 0.01
self._wcoord_sliders: list[kw.Slider] = []
with_ik = len(self.context.multibody.enabledConstraints()) > 0
# callback to change subhinge coords set by the sliders
def changeCoord(sh, st, scene, Q, cindex, with_ik, wstatus=None):
# print("SSS", shindex, cindex, bd)
if not sh:
return
if 1:
# print("TTTT", shindex, cindex, hge.nSubhinges(), item.name(), Q)
if not cindex < sh.nQ():
raise ValueError(
f"The {cindex} coordinate index should be less than {sh.nQ()} - the number of coordinates for the {sh.name()} subhinge"
)
Qvec = sh.getQ()
Qvec[cindex] = Q
sh.setQ(Qvec)
if with_ik:
# do IK
# offset = st.coordOffsets(sh).Q
st.cks().freezeCoord(sh, cindex, kd.CKFrozenCoordType.Q)
err = st.cks().solveQ()
if err < 1e-10:
color = "green"
stxt = "SUCCESS"
extra = f"[Q={Q:.4}]"
else:
color = "red"
stxt = "FAILED"
extra = f"[Q={Q:.4}, err={err:.4e}]"
status = f'**IK status:** <span style="color:{color}">{stxt}</span> {extra}'
# print(" err=", err, color)
st.cks().unfreezeCoord(sh, cindex, kd.CKFrozenCoordType.Q)
else:
status = "**IK status:** N/A"
if wstatus:
wstatus.setText(status)
scene.update()
for cindex in range(3):
self._wcoord_sliders.append(
kw.Slider(
router,
text=f"Coord {cindex}",
# need to use the shindes=shindex etc syntax to
# avoid the variable itself beig capture by the
# lambda (else only get the last value)
on_change=lambda Q, cindex=cindex: changeCoord(
self.item,
self.context.multibody,
self.context.scene,
Q,
cindex,
with_ik,
self._wmd_coord_ik,
),
opts=slider_opts,
# tooltip="Change the scaling of the stick parts parts for the body in 3D graphics"
)
)
self._wcoord_sliders[-1].setValue(0)
self.wroot.addChild(self._wmd_coord_ik)
for cindex in range(3):
self.wroot.addChild(self._wcoord_sliders[cindex])
def setup(self, item: kd.PhysicalSubhinge, item_context: kw.Json, /):
super().setup(item, item_context)
"""
_highlightFrames(
[item.oframe(), item.pframe()], toggle_over_set=False, gui_context=self.context
)
"""
# enable subhinge/coord sliders for the coords available for the body
for cindex in range(3):
if cindex < item.nQ():
slider = self._wcoord_sliders[cindex]
slider.setVisible(True)
slider.setValue(item.getQ()[cindex])
else:
self._wcoord_sliders[cindex].setVisible(False)
# print("JJJJ", shindex, cindex)
class FramePairHingeWidgets:
"""Class to create frame pair hinge widgets."""
def __init__(self, gui_context: GuiContext, group_tag=""):
router = gui_context.router
self.context = gui_context
# self.item = None
# ----------------------------------
# the initial Q values (to use for reset)
self.init_Q = np.array([])
self.full_Q = np.array([])
# --------------------------------------
# create subhinge and coordinate entries
# set IK mode on/off for coordinates
self._coord_ik = False
def _toggleCoordIK(cstate):
self._coord_ik = cstate
self._wmd_coord_ik_status.setVisible(cstate)
self._wcoord_ik = kw.Toggle(
router,
text="IK mode",
on_toggle=_toggleCoordIK,
tooltip="Toggle constraint IK mode for the slider coordinates mode",
render_as_button=True,
)
# markdown to show status from IK
self._wmd_coord_ik_status = kw.Markdown(router, text="**IK status:**")
import math
def _toggleCoordMove(cstate, shindex, cindex):
if cstate:
# reset all the other move buttons
# print("HHH", shindex, cindex)
for shi in range(6):
for ci in range(3):
if shi == shindex and ci == cindex:
continue
else:
# pass
self._wcoord_buttons[shi * 3 + ci].setValue(False)
# record the indices for the selecting coordinate
self._coord_move_indices = [shindex, cindex]
# change the range of the slider based on the subhinge joint limits
hge = self.item
# print("JJJJ", shindex, cindex, hge.nSubhinges())
if not shindex < hge.nSubhinges():
raise ValueError(
f"The {shindex} sughinge index should be less than {hge.nSubhinges()} - the number of subhinges in the {hge.name()} hinge"
)
sh = hge.subhinge(shindex)
nQ = sh.nQ()
if not cindex < nQ:
raise ValueError(
f"The {cindex} coordinate index should be less than {nQ()} - the number of coordinates for the {sh.name()} subhinge"
)
slider = self._wcoord_move_slider
slider.setEnabled(True)
self._wswing_kinsim.setEnabled(True)
if isinstance(sh, kd.Physical1DofSubhinge):
minQ, maxQ = sh.getJointLimits()
if not math.isnan(minQ):
slider.setMin(minQ)
if not math.isnan(maxQ):
slider.setMax(maxQ)
slider.setValue(sh.getQ()[0])
else:
self._coord_move_indices = [None, None]
# reset the slider limits
slider = self._wcoord_move_slider
slider.setEnabled(False)
slider.setMin(-3.2)
slider.setMax(3.2)
self._wcoord_move_slider.setEnabled(False)
self._wswing_kinsim.setEnabled(False)
# slider to change coordinates
slider_opts = kw.SliderOptions()
slider_opts.min = -3.2
slider_opts.max = 3.2
slider_opts.step = 0.01
slider_opts.tooltip = "Set the selected coordinate's value"
# the one slider to use to articulate the specified button
self._wcoord_move_slider = kw.Slider(
router,
# text=f'<span style="color:#FF5733">Subhinge {shindex}/Coord {cindex}</span>',
text=f"Set Q",
on_change=lambda Q: changeSubhingeCoord(
self.item,
self.subtree,
self.context.scene,
Q,
self._coord_move_indices[0], # subhinge index
self._coord_move_indices[1], # coord index
self._coord_ik,
self._wmd_coord_ik_status,
),
opts=slider_opts,
# tooltip="Change the scaling of the stick parts parts for the body in 3D graphics"
)
# slider.setValue(0)
self._wlayout_coord_ik = widgetArray(
router,
label="Manually change coordinate",
children=[self._wcoord_move_slider, self._wcoord_ik, self._wmd_coord_ik_status],
kind="accordion", #
accordion_group_tag=group_tag,
)
self._wlayout_coord_ik.setTooltip("Manually change the body's selected hinge coordiante")
self._wlayout_coord_ik.setOpen(True)
# self._wik_layout.addChild(self._wlayout_coord_ik)
# -------------------------------------
# free swing, no IK
self._wswing = kw.Button(
router,
text="Swing free",
on_press=lambda: _swingHinge(self.subtree, [self.item], True, self.context),
tooltip="Auto articulate the body while ignoring any constraints",
)
# constrained swing, with IK on
self._wswing_ik = kw.Button(
router,
text="Swing IK",
on_press=lambda: _swingHinge(self.subtree, [self.item], False, self.context),
tooltip="Auto articulate the body with constraint inverse kinematics",
)
def _kinSim():
shindex = self._coord_move_indices[0]
cindex = self._coord_move_indices[1]
if shindex is None or cindex is None:
return
_mbodyKinematicsSim(
self.item.subhinge(shindex),
cindex,
0.3,
0.5,
self.context,
)
# constrained swing, with IK on
self._wswing_kinsim = kw.Button(
router,
text="Kinematics sim",
on_press=_kinSim,
tooltip="Articulate the body using kinematics simulation mode",
)
self._wlayout_articulate = widgetArray(
router,
label="Auto swing coordinate",
children=[self._wswing, self._wswing_ik, self._wswing_kinsim],
kind="accordion",
alignment="row",
accordion_group_tag=group_tag,
)
self._wlayout_articulate.setTooltip("Articulate the body's selected hinge coordinate")
# self._wik_layout.addChild(self._wlayout_articulate)
# self.wroot.addChild(self._wik_layout)
# -----------------------
"""
self._wcoord_ik = kw.Button(
router,
text="Toggle coord IK",
on_press=lambda: _toggleCoordIK(),
tooltip="Toggle constraint IK mode for the slider coordinates mode",
)
"""
# self._wmd_coord_ik = kw.Markdown(router, text="**Articulate**", in_line=True)
# markdown to show status from IK
# self._wmd_coord_ik_status = kw.Markdown(router, text="**IK status:**")
# variable to track which subhinge/coordinate indices have been
# chosen for motion via the slider
self._coord_move_indices: list[int | None] = [None, None]
# callback to reset the selected coordinate
def _localCoordReset():
shindex = self._coord_move_indices[0]
cindex = self._coord_move_indices[1]
if shindex is None or cindex is None:
return
hge = self.item
sh = hge.subhinge(shindex)
Q = hge.coordData().getQ()
offset = hge.coordData().coordOffsets(sh)
val = self.init_Q[offset.Q + cindex]
Q[offset.Q + cindex] = val
hge.coordData().setQ(Q)
# reset the slider state also
self._wcoord_move_slider.setValue(val)
self.context.scene.update()
self._wcoord_local_reset = kw.Button(
router,
text="Single coordinate",
on_press=_localCoordReset,
tooltip="Reset the selected coordinate to original value",
# render_as_button=True,
)
# callback to reset all the coordinates
def _fullCoordReset():
gui_context.multibody.setQ(self.full_Q)
gui_context.scene.update()
self._wcoord_full_reset = kw.Button(
router,
text="All coordinates",
on_press=_fullCoordReset,
tooltip="Reset all the coordinates to the original value",
# render_as_button=True,
)
self._wlayout_coord_reset = widgetArray(
router,
label="Coord reset",
children=[
self._wcoord_local_reset,
self._wcoord_full_reset,
], # self._wmd_coord_ik_status],
kind="accordion", # "inputgroup",
)
self._wlayout_coord_reset.setTooltip("Reset the hinge coordinates")
# self._wik_layout.addChild(self._wlayout_coord_status)
# --------------------------
# create buttons for max of 6 subhinges, with each having max 3
# coords. we only make visible the ones that are applicable for
# a body
self._wcoord_buttons: list[kw.Toggle] = [None] * 18 # pyright: ignore
for shindex in range(6):
for cindex in range(3):
coord_button = self._wcoord_buttons[shindex * 3 + cindex] = kw.Toggle(
router,
text=f"Coord[{cindex}]",
on_toggle=lambda cstate, shindex=shindex, cindex=cindex: _toggleCoordMove(
cstate, shindex, cindex
),
tooltip=f"Select the subhinge {shindex}/coordinate index {cindex} for motion",
render_as_button=True,
)
coord_button.setValue(False)
# ---------------------------
def setAxesCB(shindex, val):
axis = eval(val)
if not isinstance(axis, Sequence):
raise ValueError(f"Expecting 3 floats for the subhinge axis - got {axis}")
if len(axis) != 3:
raise ValueError(f"Expecting 3 floats for the subhinge axis - got {len(axis)}")
naxis = np.array(axis)
nrm = np.linalg.norm(naxis)
if nrm == 0:
raise ValueError(f"Expecting non-zero 3-vector for the subhinge axis - got {axis}")
sh = cast(kd.Physical1DofSubhinge, self.item.subhinge(shindex))
if not sh.nU() == 1:
raise ValueError(
f"Can set subhinge axis for only 1 dof subhinges, and not for one with {sh.nU()} dofs."
)
vaxis = naxis / nrm
sh.setUnitAxis(vaxis)
astr = np.array2string(vaxis, separator=", ")
self.shaxes[shindex].setValue(astr)
def setLimitsCB(shindex, val):
limits = eval(val)
assert isinstance(limits, Sequence)
assert len(limits) == 2
if not isinstance(axis, Sequence):
raise ValueError(f"Expecting 2 floats for the joint limits - got {limits}")
if len(axis) != 2:
raise ValueError(f"Expecting 2 floats for the joint limits - got {len(limits)}")
if np.isnan(limits[0]):
raise ValueError(
f"Expecting legal float values for the joint limits - got {limits}"
)
if np.isnan(limits[1]):
raise ValueError(
f"Expecting legal float values for the joint limits - got {limits}"
)
if not limits[0] <= limits[1]:
raise ValueError(
f"Expecting first joint limit to be smaller than the second - got {limits}"
)
sh = cast(kd.Physical1DofSubhinge, self.item.subhinge(shindex))
# assert sh.nU() == 1
if not sh.nU() == 1:
raise ValueError(
f"Can set joint limits for only 1 dof subhinges, and not for one with {sh.nU()} dofs."
)
nlimits = np.array(limits)
sh.setJointLimits(nlimits)
astr = np.array2string(nlimits, separator=", ")
self.shlimits[shindex].setValue(astr)
self.shrows: list[kw.Layout | kw.InputGroup | kw.Accordion] = [None] * 6 # pyright: ignore
self.shaxes: list[kw.StringInput] = [None] * 6 # pyright: ignore
self.shlimits: list[kw.StringInput] = [None] * 6 # pyright: ignore
w_shrows = []
for shindex in range(6):
wbtns = []
for cindex in range(3):
# self._wlayout_coord_ik.addChild(self._wcoord_sliders[shindex * 3 + cindex])
# self._wlayout_coord_ik.addChild(self._wcoord_buttons[shindex * 3 + cindex])
# w_shrow.addChild(self._wcoord_buttons[shindex * 3 + cindex])
wbtns.append(self._wcoord_buttons[shindex * 3 + cindex])
# self.wroot.addChild(self._wcoord_buttons[shindex * 3 + cindex])
waxis = kw.StringInput(
router,
"Change axis",
on_change=lambda val, shindex=shindex: setAxesCB(shindex, val),
)
waxis.setTooltip("The unit norm subhinge axis vector")
waxis.setSizeClass(kw.SizeClass.WIDE)
wlimits = kw.StringInput(
router,
"Joint limits",
on_change=lambda val, shindex=shindex: setLimitsCB(shindex, val),
)
wlimits.setTooltip("The joint limits for the 1 dof subhinge")
wlimits.setSizeClass(kw.SizeClass.WIDE)
# TODO - should not need to create this explicitly and should do
# so as a string. However it is disappearing if we do not create
# explicitly
# wmd_shrow = kw.Markdown(router, text=f" **Subhinge[{shindex}]**", in_line=True)
w_shrow = widgetArray(
router,
label=f" **Subhinge[{shindex}]**", # wmd_shrow,
children=wbtns + [waxis, wlimits],
)
w_shrow.setTooltip("Coordinate selection, and related widgets")
# self.shrows[shindex] = (w_shrow, wmd_shrow)
self.shrows[shindex] = w_shrow
self.shaxes[shindex] = waxis
self.shlimits[shindex] = wlimits
# self._wlayout_move_buttons.addChild(w_shrow)
w_shrows.append(w_shrow)
self._wlayout_move_buttons = widgetArray(
router,
label="Select move coordinate",
children=w_shrows,
alignment="column",
alignItems="left",
kind="inputgroup",
)
# self._wik_layout.addChild(self._wlayout_move_buttons)
self._wlayout_move_buttons.setVisible(True)
self._wlayout_move_buttons.setTooltip("Coordinate selection buttons")
self._wik_layout = widgetArray(
router,
[
self._wlayout_move_buttons, # select the coordinate
self._wlayout_coord_ik, # manually move the coordinate
self._wlayout_articulate, # swing the coordinate
self._wlayout_coord_reset, # reset the states
],
label="Kinematics",
alignment="column",
alignItems="left",
kind="accordion",
)
self._wik_layout.setTooltip("Controls for changing the body's hinge coordinates")
@property
def subtree(self):
st = self._subtree_ref()
if not st:
raise ValueError("SubTree reference has been removed.")
return st
@subtree.setter
def subtree(self, st: kd.SubTree):
self._subtree_ref = kc.CppWeakRef(st)
@property
def item(self):
if self._item_ref is None:
raise RuntimeError("The associated FramePairHinge has gone out of scope.")
item = self._item_ref()
if item is None:
raise RuntimeError("The associated FramePairHinge has gone out of scope.")
return item
@item.setter
def item(self, item: kd.FramePairHinge | kc.CppWeakRef[kd.FramePairHinge] | None):
if isinstance(item, kd.FramePairHinge):
self._item_ref = kc.CppWeakRef(item)
else:
self._item_ref = item
def setup(self, itemref: kc.CppWeakRef[kd.FramePairHinge] | None, stref: kc.CppWeakRef):
self.item = itemref
self._subtree_ref = stref # self.subtree
st = stref()
if itemref is None:
item = None
else:
item = itemref()
not_root_body = item is not None
is_not_locked = not_root_body and item.hingeType() != kd.HingeType.LOCKED
has_constraints = isinstance(st, kd.SubGraph) and len(st.enabledConstraints()) > 0
if not_root_body:
self.init_Q = item.coordData().getQ()
self.full_Q = self.context.multibody.getQ()
# print("MMM", has_constraints, self.subtree.name())
self._wlayout_coord_ik.setVisible(is_not_locked)
self._wcoord_ik.setVisible(has_constraints)
# hide IK status until IK mode is enabled
self._wmd_coord_ik_status.setVisible(False)
# show the toggle IK mode button only if we have constraints
self._wik_layout.setVisible(not_root_body and is_not_locked)
# self._wswing.setVisible(not_root_body)
self._wswing_ik.setVisible(not_root_body and has_constraints)
self._wlayout_articulate.setVisible(is_not_locked)
if is_not_locked:
hge = cast(kd.FramePairHinge, item) # .parentHinge()
self._wlayout_move_buttons.setVisible(hge.coordData().nQ() != 0)
for shindex in range(6):
for cindex in range(3):
if shindex < hge.nSubhinges():
sh = hge.subhinge(shindex)
nQ = sh.nQ()
self.shrows[shindex].setVisible(nQ != 0)
self.shaxes[shindex].setVisible(nQ == 1)
self.shlimits[shindex].setVisible(nQ == 1)
if nQ == 1:
sh1 = cast(kd.Physical1DofSubhinge, sh)
astr = np.array2string(sh1.getUnitAxis(), separator=", ")
self.shaxes[shindex].setValue(astr)
astr = np.array2string(sh1.getJointLimits(), separator=", ")
self.shlimits[shindex].setValue(astr)
if cindex < nQ:
# slider = self._wcoord_sliders[shindex * 3 + cindex]
button = self._wcoord_buttons[shindex * 3 + cindex]
button.setValue(False)
"""
if nQ == 1:
minQ, maxQ = sh.getJointLimits()
if not math.isnan(minQ):
slider.setMin(minQ)
if not math.isnan(maxQ):
slider.setMax(maxQ)
slider.setVisible(True)
slider.setValue(hge.subhinge(shindex).getQ()[cindex])
"""
# slider.setVisible(False)
button.setVisible(True)
else:
# self._wcoord_sliders[shindex * 3 + cindex].setVisible(False)
self._wcoord_buttons[shindex * 3 + cindex].setVisible(False)
else:
# self._wcoord_sliders[shindex * 3 + cindex].setVisible(False)
# self._wcoord_buttons[shindex * 3 + cindex].setVisible(False)
self.shrows[shindex].setVisible(False)
# print("JJJJ", shindex, cindex)
# enable the first button
self._wcoord_buttons[0].setValue(True, trigger_own_callback=True)
self._coord_move_indices = [0, 0]
class CompoundHingeWidgets:
"""Class to create compound hinge widgets."""
def __init__(self, gui_context: GuiContext):
router = gui_context.router
self.context = gui_context
# self.item = None
# variable to track which subhinge/coordinate indices have been
# chosen for motion via the slider
self._coord_move_indices = [None, None]
# ----------------------------------
# the initial Q values (to use for reset)
self.init_Q = np.array([])
self.full_Q = np.array([])
# variable to track which coordinate index has been
# chosen for motion via the slider
self._coord_move_index = 0
# ------------------------------------
self.max_coord = 20
def _toggleCoordMove(cstate: bool, cindex: int):
if cstate:
# reset all the other move buttons
for ci in range(self.max_coord):
if ci == cindex:
continue
else:
# pass
self._wcoord_buttons[ci].setValue(False)
# record the indices for the selecting coordinate
self._coord_move_index = cindex
# change the range of the slider based on the subhinge joint limits
hge = self.item
# print("JJJJ", shindex, cindex, hge.nSubhinges())
sh = hge.subhinge(0)
nQ = sh.nQ()
if not cindex < nQ:
raise ValueError(
f"The {cindex} coordinate index should be less than {sh.nQ()} - the number of coordinates for the {sh.name()} subhinge"
)
slider = self._wcoord_move_slider
self._wcoord_move_slider.setEnabled(True)
self._wswing_kinsim.setEnabled(True)
"""
if nQ == 1:
minQ, maxQ = sh.getJointLimits()
if not math.isnan(minQ):
slider.setMin(minQ)
if not math.isnan(maxQ):
slider.setMax(maxQ)
"""
else:
self._coord_move_index = None
# reset the slider limits
slider = self._wcoord_move_slider
slider.setMin(-3.2)
slider.setMax(3.2)
self._wcoord_move_slider.setEnabled(False)
self._wswing_kinsim.setEnabled(False)
# slider to change coordinates
slider_opts = kw.SliderOptions()
slider_opts.min = -3.2
slider_opts.max = 3.2
slider_opts.step = 0.01
slider_opts.tooltip = "Set the selected coordinate's value"
# the one slider to use to articulate the specified button
self._wcoord_move_slider = kw.Slider(
router,
# text=f'<span style="color:#FF5733">Subhinge {shindex}/Coord {cindex}</span>',
text=f"Set Q",
on_change=lambda Q: changeSubhingeCoord(
self.item,
self.subtree,
self.context.scene,
Q,
0, # subhinge index
self._coord_move_index, # coord index
False, # self._coord_ik,
None, # self._wmd_coord_ik_status,
),
opts=slider_opts,
# tooltip="Change the scaling of the stick parts parts for the body in 3D graphics"
)
# slider.setValue(0)
self._wlayout_coord_ik = widgetArray(
router,
# label=self._wmd_coord_ik_status,
children=[
self._wcoord_move_slider,
],
)
# self._wik_layout.addChild(self._wlayout_coord_ik)
# ---------------------------------
# callback to reset the selected coordinate
def _localCoordReset():
cindex = self._coord_move_index
hge = self.item
Q = hge.coordData().getQ()
# offset = hge.coordData().coordOffsets(sh)
val = self.init_Q[cindex]
Q[cindex] = val
hge.coordData().setQ(Q)
# reset the slider state also
self._wcoord_move_slider.setValue(val)
self.context.scene.update()
self._wcoord_local_reset = kw.Button(
router,
text="Coord reset",
on_press=_localCoordReset,
tooltip="Reset the selected coordinate to original value",
)
# callback to reset all the coordinates
def _fullCoordReset():
self.context.multibody.setQ(self.full_Q)
self.context.scene.update()
self._wcoord_full_reset = kw.Button(
router,
text="Full reset",
on_press=_fullCoordReset,
tooltip="Reset all the coordinates to the original value",
)
self._wlayout_coord_reset = widgetArray(
router,
label="Coord Reset", # self._wmd_coord_ik_status,
children=[
self._wcoord_local_reset,
self._wcoord_full_reset,
], # , self._wmd_coord_ik_status],
kind="accordion", # "inputgroup",
)
# self._wik_layout.addChild(self._wlayout_coord_status)
self._wlayout_coord_reset.setTooltip("Reset the compound hinge coordinates")
# ---------------------------
# free swing, no IK
self._wswing = kw.Button(
router,
text="Swing",
on_press=lambda: _swingHinge(
self.item.compoundBody().bodiesTree().parentSubTree(),
[self.item],
False,
self.context,
),
tooltip="Auto articulate the compound body",
)
# ------------------------------------
# constrained swing, with IK on
self._wswing_kinsim = kw.Button(
router,
text="Kinematics sim",
on_press=lambda: _ceKinematicsSim(
cast(kd.SubGraph, self.item.compoundBody().bodiesTree().parentSubTree()),
self.item.subhinge(0),
self._coord_move_index,
0.3,
0.5,
self.context,
),
tooltip="Articulate the CE compound body using kinematics sim",
)
# self._wmd_articulate = kw.Markdown(router, text="**Articulate**", in_line=True)
self._wlayout_articulate = widgetArray(
router,
label="Articulate",
children=[self._wswing, self._wswing_kinsim],
kind="inputgroup",
)
# self._wik_layout.addChild(self._wlayout_articulate)
# --------------------------------------
# create subhinge and coordinate entries
rowlen = 6
rowindex = 0
cindex = 0
# -------------------------------
# create buttons for all the coordinates
# we only make visible the ones that are applicable for the CE compound body
self._wcoord_buttons: list[kw.Toggle] = [ # pyright: ignore
None
] * self.max_coord # array with all the buttons
for cindex in range(self.max_coord):
coord_button = self._wcoord_buttons[cindex] = kw.Toggle(
router,
text=f"Coord[{cindex}]",
on_toggle=lambda cstate, cindex=cindex: _toggleCoordMove(cstate, cindex),
tooltip=f"Select the coordinate index {cindex} for motion",
render_as_button=True,
)
coord_button.setValue(False)
self.shrows: list[kw.Layout | kw.InputGroup] = [None] * (int(self.max_coord / 6) + 1) # pyright: ignore
w_shrows = []
cindex = 0
while rowindex < self.max_coord / rowlen:
wbtns = []
ci = 0
while ci < 6 and cindex < self.max_coord:
# self._wlayout_coord_ik.addChild(self._wcoord_sliders[shindex * 3 + cindex])
# self._wlayout_coord_ik.addChild(self._wcoord_buttons[shindex * 3 + cindex])
# w_shrow.addChild(self._wcoord_buttons[shindex * 3 + cindex])
wbtns.append(self._wcoord_buttons[cindex])
cindex += 1
ci += 1
# self.wroot.addChild(self._wcoord_buttons[shindex * 3 + cindex])
# TODO - should not need to create this explicitly and should do
# so as a string. However it is disappearing if we do not create
# explicitly
# wmd_shrow = kw.Markdown(router, text=" **GGGGG*", in_line=True)
w_shrow = widgetArray(router, children=wbtns)
w_shrows.append(w_shrow)
self.shrows[rowindex] = w_shrow
# self.wroot.addChild(w_shrow)
# self._wlayout_move_buttons.addChild(w_shrow)
# w_shrow.setVisible(True)
rowindex += 1
self._wlayout_move_buttons = widgetArray(
router,
label="Select move coordinate",
children=w_shrows,
alignment="column",
alignItems="left",
kind="inputgroup",
)
# self._wik_layout.addChild(self._wlayout_move_buttons)
self._wlayout_move_buttons.setVisible(True)
self._wlayout_move_buttons.setTooltip("Coordinate selection buttons")
self._wik_layout = widgetArray(
router,
[
# self._wlayout_articulate
self._wlayout_move_buttons, # select the coordinate
self._wlayout_coord_ik, # manually move the coordinate
self._wlayout_articulate, # swing the coordinate
self._wlayout_coord_reset, # reset the states
],
label="Kinematics",
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
# self.wroot.addChild(self._wik_layout)
self._wik_layout.setTooltip("Controls for changing the compound body's hinge coordinates")
@property
def subtree(self):
return self.item.compoundBody().bodiesTree().parentSubTree()
@property
def item(self):
if self._item_ref is None:
raise RuntimeError("The associated CompoundHinge has gone out of scope.")
item = self._item_ref()
if item is None:
raise RuntimeError("The associated CompoundHinge has gone out of scope.")
return item
@item.setter
def item(self, item: kd.CompoundHinge | kc.CppWeakRef[kd.CompoundHinge] | None):
if isinstance(item, kd.CompoundHinge):
self._item_ref = kc.CppWeakRef(item)
else:
self._item_ref = item
def setup(
self,
itemref: kc.CppWeakRef[kd.CompoundHinge],
# stref: kc.CppWeakRef
# _: kw.Json,
/,
):
self.item = itemref
hge = self.item
sh = hge.subhinge(0)
nQ = sh.nQ()
self._wlayout_move_buttons.setVisible(nQ != 0)
max_rowindex = int(nQ / 6) + 1
# print(f"nQ={nQ}, masrow={max_rowindex}")
for cindex in range(self.max_coord):
rowindex = int(cindex / 6)
button = self._wcoord_buttons[cindex]
if cindex < nQ:
# slider = self._wcoord_sliders[shindex * 3 + cindex]
button.setValue(False)
"""
if nQ == 1:
minQ, maxQ = sh.getJointLimits()
if not math.isnan(minQ):
slider.setMin(minQ)
if not math.isnan(maxQ):
slider.setMax(maxQ)
slider.setVisible(True)
slider.setValue(hge.subhinge(shindex).getQ()[cindex])
"""
# slider.setVisible(False)
self.shrows[rowindex].setVisible(True)
button.setVisible(True)
# self.shrows[cindex].setVisible(False)
else:
# self._wcoord_sliders[shindex * 3 + cindex].setVisible(False)
if rowindex >= max_rowindex:
self.shrows[rowindex].setVisible(False)
pass
button.setVisible(False)
# self.shrows[cindex].setVisible(False)
# enable the first button -- it's ok to use state set bc we already have item ref set
self._wcoord_buttons[0].setValue(True, trigger_own_callback=True)
# self.shrows[0].setVisible(True)
self.init_Q = hge.coordData().getQ()
self.full_Q = self.context.multibody.getQ()
class DynamicsSimWidgets:
"""Class to create dynamics sim related widgets."""
def __init__(self, gui_context: GuiContext):
router = gui_context.router
self.context = gui_context
# self.item = None
# constrained swing, with IK on
self._external_sp = None
self._dyn_sp: kd.StatePropagator | None = None
self._dyn_integ_type = ki.IntegratorType.CVODE # default
# self._dyn_init_U = 0
self._dyn_grav_accel: NDArray[np.float64] | None = None
self._dyn_contact_force: kcoll.ContactForceBase | None = None
# self._dyn_duration = 0.5 # same as self._advance_incr
# -----------------------
# ----------------------------
adv_opts = kw.SliderOptions()
adv_opts.min = -3
adv_opts.max = 3
# 10 steps per decade
adv_opts.step = 0.5
# Use log scale on slider
adv_opts.log_scale = True
self._advance_incr = 0.1
def setincr(new_incr, units):
incr_seconds = (new_incr * ureg(units)).to("s")
self._advance_incr = incr_seconds.magnitude
self._wadvinput = kw.QuantityInput(router, "Duration", setincr)
# Default to 0.1 s
self._wadvinput.state("quantity_state").set({"value": 0.1, "units": "s"})
"""
def setDuration(new_incr, _):
# incr_seconds = (new_incr * ureg(units)).to("s")
self._dyn_duration = new_incr
self._wdyn_duration = kw.QuantityInput(router, "Duration", setDuration)
# Default to 0.1 s
self._wdyn_duration.state("quantity_state").set({"value": 0.5, "units": "s"})
"""
# --------
color = "green"
stxt = "Idle"
status = f'**Status:** <span style="color:{color}">{stxt}</span>'
self._wmd_status = kw.Markdown(router, text=status)
# Advance by increment rounded to the nearest nanosecond
self._wadvbtn = kw.Button(
router,
text="Advance by",
on_press=lambda: createAdvanceByCallback(
# self.item, np.timedelta64(int(self._advance_incr * 1e9), "ns")
self._dyn_sp,
self.context,
np.timedelta64(int(self._advance_incr * 1e9), "ns"),
self._wmd_status,
),
)
# ----------------------------
self._wpause = kw.Button(
router, text="\u23f8/\u25b6", on_press=lambda: createPauseCb(self._dyn_sp)()
)
def _stopCB(sp):
assert sp
sp.stop()
self._wstop = kw.Button(
router,
text="\u23f9",
# on_press=lambda: self._dyn_sp.stop())
on_press=lambda: _stopCB(self._dyn_sp),
)
# self._wmd_pause = kw.Markdown(router, text="**Sim run**", in_line=True)
self._wlayout_pause = widgetArray(
router,
label="**Run sim**",
children=[self._wadvinput, self._wadvbtn, self._wpause, self._wstop, self._wmd_status],
kind="layout", # "inputgroup",
)
# -------------
def _stepSizeCB(sp, new_val):
assert sp
sp.setMaxStepSize(new_val)
slider_opts = kw.SliderOptions()
slider_opts.min = -6
slider_opts.max = 2
slider_opts.step = 1
slider_opts.log_scale = True
self._wmaxstep_slider = kw.Slider(
router,
"Max step size",
# lambda new_val: self._dyn_sp.setMaxStepSize(new_val),
lambda new_val: _stepSizeCB(self._dyn_sp, new_val),
slider_opts,
)
self._wmaxstep_slider.setSizeClass(kw.SizeClass.MEDIUM)
# ----------------------------
# Mapping of indices to integrator type
self.integ_types = [
ki.IntegratorType.RK4,
ki.IntegratorType.CVODE,
ki.IntegratorType.CVODE_STIFF,
]
def integSelect(sp, index):
"""Set the integrator according to a given index."""
integ_type = self.integ_types[index]
self._dyn_integ_type = integ_type
# old_type = sp.getIntegrator().getIntegratorType()
# if old_type != integ_type:
# old_is_cvode = old_type in [
# ki.IntegratorType.CVODE,
# ki.IntegratorType.CVODE_STIFF,
# ki.IntegratorType.CVODE_NEWTON,
# ]
# new_is_cvode = integ_type in [
# ki.IntegratorType.CVODE,
# ki.IntegratorType.CVODE_STIFF,
# ki.IntegratorType.CVODE_NEWTON,
# ]
# if old_is_cvode and new_is_cvode:
# sp.setIntegrator(integ_type, sp.getIntegrator().getOptions())
# else:
sp.setIntegrator(integ_type)
self._winteg = kw.Dropdown(
router,
"",
["RK4", "CVode", "CVode Stiff"],
lambda index: integSelect(self._dyn_sp, index),
)
def createIntegratorPanel(sp):
assert sp
integ = sp.getIntegrator()
selection = kw.Selection(items=[kw.Selection.Item(id=integ.id(), context={})])
integ_panel = self.context.setup_info_panel(selection)
# integ_panel.selection.onChange(lambda x: None)
integ_panel.updateFor(integ, {})
"""
self.dock.addChild(
title="Integrator Info",
widget=integ_panel.wroot,
relative_to=self.mbody_tree_view,
direction="below",
)
"""
self.context.dock.addChild(
"Integrator Info",
integ_panel.wroot,
# self.wroot,
self.context.visjs_iframe,
"within",
)
self._winteg_panel = kw.Button(
router,
text="Integrator panel",
on_press=lambda: createIntegratorPanel(self._dyn_sp),
tooltip="Create a separate info panel for the integrator",
)
# self._wmd_integ = kw.Markdown(router, text="**Integrator**", in_line=True)
self._wlayout_integ = widgetArray(
router,
label="**Integrator**",
children=[self._winteg, self._winteg_panel, self._wmaxstep_slider],
kind="layout", # "accordion", # "inputgroup",
)
# ----------------------------
def _setupDynSimCB(cstate):
if not kc.allReady():
print("WARNING: kc.allReady() is failing - call mb.resetData()")
return
# assert kc.allReady()
# self._wdynsim_run.setEnabled(cstate)
self._wadvbtn.setEnabled(cstate)
self._wenable_gravity.setEnabled(not cstate)
self._wenable_contact.setEnabled(not cstate)
self._wcoord_full_reset.setEnabled(cstate)
self._wmaxstep_slider.setEnabled(cstate)
self._wsp_reset.setEnabled(cstate)
if cstate:
"""
if self._dyn_grav_accel is None:
raise ValueError("Gravity not set.")
if self._dyn_contact_force is None:
raise ValueError("Dynamic contact force not set.")
"""
self._dyn_sp = _subTreeSim(
self.item,
integ_type=self._dyn_integ_type,
# init_U=self._dyn_init_U,
gravity_accel=self._dyn_grav_accel,
contact_force=self._dyn_contact_force,
gui_context=self.context,
)
self._winteg_panel.setEnabled(True)
else:
self._dyn_sp = None
self._winteg_panel.setEnabled(False)
self._wdynsim_setup = kw.Toggle(
router,
text="Setup",
on_toggle=lambda cstate: _setupDynSimCB(cstate),
tooltip="Set up dynamics simulation",
render_as_button=True,
)
def _enableContact(cstate):
if cstate:
hc = kcoll.HuntCrossleyContactForce("hunt_crossley_contact")
hc.params.kp = 1e4
hc.params.kc = 5e3
hc.params.mu = 0.7
hc.params.n = 1.0
hc.params.linear_region_tol = 1e-3
self._dyn_contact_force = hc
else:
self._dyn_contact_force = None
self._wenable_contact = kw.Toggle(
router,
text="Contact",
on_toggle=lambda cstate: _enableContact(cstate),
tooltip="Enable contact dynamics in dynamics sim",
render_as_button=True,
)
def _enableGravity(cstate):
self._dyn_grav_accel = np.array([0, 0, -3.73]) if cstate else None
self._wenable_gravity = kw.Toggle(
router,
text="Gravity",
on_toggle=lambda cstate: _enableGravity(cstate),
tooltip="Enable gravity in dynamics sim",
render_as_button=True,
)
# -----------------
self._wlayout_setup = widgetArray(
router,
label="**Setup propagator**",
children=[
self._wdynsim_setup,
self._wenable_gravity,
self._wenable_contact,
# self._wdynsim_run,
# self._wdyn_duration,
# self._winit_u,
],
kind="layout", # "inputgroup",
)
# --------------------
def setInitU(new_incr):
# self._dyn_init_U = new_incr
self.item.setU(new_incr)
if isinstance(self.item, kd.SubGraph) and self.item.enabledConstraints():
self.item.cks().solveU()
self._winit_u = kw.FloatInput(
router,
"Init U",
"The initial generalized velocity to set for the coordinates",
on_change=setInitU,
)
self._winit_u.setSizeClass(kw.SizeClass.MEDIUM)
# self._winit_u.state("quantity_state").set({"value": 0.0, "units": "s"})
self._winit_u.setStep(0.01)
self._winit_u.setMin(-1)
self._winit_u.setMax(1)
self._winit_u.setValue(0)
def _fullCoordReset():
assert self._dyn_sp
self.item.setQ(self.full_Q)
self.item.setU(self.full_U)
x = self._dyn_sp.assembleState()
self._dyn_sp.setState(x)
self._dyn_sp.setTime(0)
self.context.scene.update()
self._wcoord_full_reset = kw.Button(
router,
text="Reset state",
on_press=_fullCoordReset,
tooltip="Reset the state to the one when this object was selected",
# render_as_button=True,
)
def _spReset():
assert self._dyn_sp
x = self._dyn_sp.assembleState()
self._dyn_sp.setState(x)
self.context.scene.update()
self._wsp_reset = kw.Button(
router,
text="SP set state",
on_press=_spReset,
tooltip="Sync the state propagator state from the SubTree's current Q/U values",
# render_as_button=True,
)
self._wlayout_reset = widgetArray(
router,
label="**Set state**",
children=[
# self._wdynsim_setup,
# self._wdyn_duration,
self._wcoord_full_reset,
self._winit_u,
self._wsp_reset,
],
kind="layout", # "inputgroup",
)
# ----------------
# -----------------------
self._dsinitfile = "subtreeState.hdf5"
def _setStateFile(x):
self._dsinitfile = x
self._wdsinitfile = kw.StringInput(
router,
"Filename",
"The state filename",
lambda x: _setStateFile(x),
rapid_submit=True,
)
self._wdsinitfile.setValue(self._dsinitfile)
def _toDSInitCB():
st = self.item
if isinstance(st, kd.SubGraph):
from Karana.Dynamics.SOADyn_types import SubGraphStateDS
sg = cast(kd.SubGraph, st)
ds = SubGraphStateDS.fromSubGraph(sg)
else:
from Karana.Dynamics.SOADyn_types import SubTreeStateDS
ds = SubTreeStateDS.fromSubTree(st)
ds.toFile(self._dsinitfile)
self._wsavedsinit = kw.Button(
router,
text="Save",
on_press=lambda: _toDSInitCB(),
tooltip="Save SubTree state to file",
)
def _fromDSInitCB():
st = self.item
if isinstance(st, kd.SubGraph):
from Karana.Dynamics.SOADyn_types import SubGraphStateDS
sg = cast(kd.SubGraph, st)
ds = SubGraphStateDS.fromFile(self._dsinitfile)
ds.toSubGraph(sg)
else:
from Karana.Dynamics.SOADyn_types import SubTreeStateDS
ds = SubTreeStateDS.fromFile(self._dsinitfile)
ds.toSubTree(st)
self.context.scene.update()
self._wloaddsinit = kw.Button(
router,
text="Load",
on_press=lambda: _fromDSInitCB(),
tooltip="Load SubTree state from file",
)
self._wlayout_state = widgetArray(
router,
label="**State to/from file**", # self._wmd_views,
children=[
self._wsavedsinit,
self._wdsinitfile,
self._wloaddsinit,
],
kind="layout", # "inputgroup",
)
# ------------------
self._wlayout_dynsim = widgetArray(
router,
label="Dynamics sim [with temporary StatePropagator]",
children=[
self._wlayout_setup,
self._wlayout_integ,
# self._wlayout_maxstep,
# self._wlayout_adv,
self._wlayout_pause,
self._wlayout_reset,
self._wlayout_state,
],
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
self._wlayout_dynsim.setTooltip(
"Do a basic dynamics simulation of the sub-tree with initial rates, gravity and contact dynamics"
)
# self.wroot.addChild(self._wlayout_dynsim)
"""
@property
def subtree(self):
return self.item.compoundBody().bodiesTree().parentSubTree()
"""
@property
def item(self):
if self._item_ref is None:
raise RuntimeError("The associated SubTree has gone out of scope.")
item = self._item_ref()
if item is None:
raise RuntimeError("The associated SubTree has gone out of scope.")
return item
@item.setter
def item(self, item: kd.SubTree | kc.CppWeakRef[kd.SubTree] | None):
if isinstance(item, kd.SubTree):
self._item_ref = kc.CppWeakRef(item)
else:
self._item_ref = item
def setup(
self, itemref: kc.CppWeakRef[kd.SubTree], spref: kc.CppWeakRef[kd.StatePropagator] | None
):
self.item = itemref
if spref:
self._dyn_sp = spref()
sp = cast(kd.StatePropagator, self._dyn_sp)
self._external_sp = True
self._wmaxstep_slider.setValue(
km.ktimeToSeconds(sp.getMaxStepSize()),
trigger_own_callback=True,
)
curr_type = sp.getIntegrator().getIntegratorType()
if curr_type in self.integ_types:
self._winteg.setIndex(self.integ_types.index(curr_type))
else:
print(f"Warning: unsupported dropdown integrator type {curr_type}")
else:
self._dyn_sp = None
self._external_sp = False
self._winteg.setIndex(self.integ_types.index(self._dyn_integ_type))
# disable widgets not meant for external state propagator
self._wlayout_setup.setVisible(not self._external_sp)
# self._wdynsim_run.setEnabled(self._external_sp)
self._wadvbtn.setEnabled(self._external_sp)
self._winteg_panel.setEnabled(self._external_sp)
self._wmaxstep_slider.setEnabled(self._external_sp)
self._wsp_reset.setEnabled(self._external_sp)
# self._wenable_gravity.setVisible(not self._external_sp)
# self._wenable_contact.setVisible(not self._external_sp)
self.full_Q = self.item.getQ()
self.full_U = self.item.getU()
"""
self._dyn_sp = None
self._wdynsim_setup.setValue(False)
self._wdynsim_run.setEnabled(False)
self._wcoord_full_reset.setEnabled(False)
self._wenable_gravity.setEnabled(True)
self._wenable_contact.setEnabled(True)
"""
class HomTranWidgets:
"""Class to create widgets to edit a HomTran."""
def __init__(self, title: str, cb: Callable, gui_context: GuiContext):
router = gui_context.router
# self.context = gui_context
# self.item = None
# --------------------------
# create roll, pitch, yaw sliders
self.xyz = [0, 0, 0]
self.rpy = [0, 0, 0]
def _updateCB():
T = self.getTransform()
cb(T)
slider_opts = kw.SliderOptions()
slider_opts.min = -3.12
slider_opts.max = 3.12
slider_opts.step = 0.01
slider_opts.log_scale = False
def _rollCB(new_val):
self.rpy[0] = new_val
quat = km.UnitQuaternion(
km.EulerAngles(self.rpy[0], self.rpy[1], self.rpy[2], km.EulerSystem.XYZ)
)
astr = np.array2string(quat.toVector4(), separator=", ")
self._wquat.setValue(astr)
_updateCB()
self._wroll = kw.Slider(
router,
"Roll",
lambda new_val: _rollCB(new_val),
slider_opts,
)
self._wroll.setSizeClass(kw.SizeClass.MEDIUM)
self._wroll.setTooltip("Roll angle (radians)")
def _pitchCB(new_val):
self.rpy[1] = new_val
quat = km.UnitQuaternion(
km.EulerAngles(self.rpy[0], self.rpy[1], self.rpy[2], km.EulerSystem.XYZ)
)
astr = np.array2string(quat.toVector4(), separator=", ")
self._wquat.setValue(astr)
_updateCB()
self._wpitch = kw.Slider(
router,
"Pitch",
lambda new_val: _pitchCB(new_val),
slider_opts,
)
self._wpitch.setSizeClass(kw.SizeClass.MEDIUM)
self._wpitch.setTooltip("Pitch angle (radians)")
def _yawCB(new_val):
self.rpy[2] = new_val
quat = km.UnitQuaternion(
km.EulerAngles(self.rpy[0], self.rpy[1], self.rpy[2], km.EulerSystem.XYZ)
)
astr = np.array2string(quat.toVector4(), separator=", ")
self._wquat.setValue(astr)
_updateCB()
self._wyaw = kw.Slider(
router,
"Yaw",
lambda new_val: _yawCB(new_val),
slider_opts,
)
self._wyaw.setSizeClass(kw.SizeClass.MEDIUM)
self._wyaw.setTooltip("Yaw angle (radians)")
# ------------------
def setQuatCB(val):
quat = eval(val)
assert isinstance(quat, Sequence)
assert len(quat) == 4
if not isinstance(axis, Sequence):
raise ValueError(f"Expecting 4 floats for a unit quaternion - got {quat}")
if len(quat) != 4:
raise ValueError(f"Expecting 4 floats for a unit quaternion - got {len(quat)}")
nquat = km.UnitQuaternion(quat)
self.rpy = nquat.toEulerAngles().angles().m
self._wxpos.setValue(self.rpy[0])
self._wypos.setValue(self.rpy[1])
self._wzpos.setValue(self.rpy[2])
_updateCB()
self._wquat = kw.StringInput(
router,
"Unit quaternion",
on_change=lambda val: setQuatCB(val),
)
self._wquat.setTooltip("The orientation unit quaternion")
self._wquat.setSizeClass(kw.SizeClass.EXTRA_WIDE)
self._wlayout_rpy = widgetArray(
router,
label="",
children=[self._wroll, self._wpitch, self._wyaw],
kind="layout",
)
self._wlayout_quat = widgetArray(
router,
label="Orientation",
children=[self._wlayout_rpy, self._wquat],
kind="inputgroup",
alignment="column",
alignItems="left",
)
# --------------------------
# create X, Y, Z sliders
slider_opts = kw.SliderOptions()
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = False
def _xposCB(new_val):
self.xyz[0] = new_val
_updateCB()
self._wxpos = kw.Slider(
router,
"X",
lambda new_val: _xposCB(new_val),
slider_opts,
)
self._wxpos.setSizeClass(kw.SizeClass.MEDIUM)
self._wxpos.setTooltip("X position (m)")
def _yposCB(new_val):
self.xyz[1] = new_val
_updateCB()
self._wypos = kw.Slider(
router,
"Y",
lambda new_val: _yposCB(new_val),
slider_opts,
)
self._wypos.setSizeClass(kw.SizeClass.MEDIUM)
self._wypos.setTooltip("Y position (m)")
def _zposCB(new_val):
self.xyz[2] = new_val
_updateCB()
self._wzpos = kw.Slider(
router,
"Z",
lambda new_val: _zposCB(new_val),
slider_opts,
)
self._wzpos.setSizeClass(kw.SizeClass.MEDIUM)
self._wzpos.setTooltip("Z position (m)")
self._wlayout_xyz = widgetArray(
router,
label="Position",
children=[self._wxpos, self._wypos, self._wzpos],
kind="inputgroup",
)
# ------------------
self._wlayout_homtran = widgetArray(
router,
label=title,
children=[
self._wlayout_xyz, # self._wlayout_rpy,
self._wlayout_quat,
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
def getTransform(self):
ee = km.EulerAngles(self.rpy[0], self.rpy[1], self.rpy[2], km.EulerSystem.XYZ)
return km.HomTran(km.UnitQuaternion(ee), self.xyz)
def setup(self, T: km.HomTran):
pos = T.getTranslation().m
self._wxpos.setValue(pos[0])
self._wypos.setValue(pos[1])
self._wzpos.setValue(pos[2])
quat = T.getUnitQuaternion()
rpy = quat.toEulerAngles(km.EulerSystem.XYZ).angles().m
self._wroll.setValue(rpy[0])
self._wpitch.setValue(rpy[1])
self._wyaw.setValue(rpy[2])
astr = np.array2string(quat.toVector4(), separator=", ")
self._wquat.setValue(astr)
class SimTranWidgets:
"""Class to create widgets to edit a HomTran+Intrinsic scale."""
def __init__(self, title: str, cb: Callable, gui_context: GuiContext):
router = gui_context.router
# --------------------------
# create X, Y, Z scale sliders
self.scale_xyz = [1, 1, 1]
self.gscale = 1
def _updateCB():
T = km.SimTran(self._whomtran.getTransform(), self.gscale)
cb(T, self.scale_xyz)
slider_opts = kw.SliderOptions()
slider_opts.min = -3
slider_opts.max = 1
slider_opts.step = 0.01
slider_opts.log_scale = True
def _gscaleCB(new_val):
self.gscale = new_val
_updateCB()
self._wgscale = kw.Slider(
router,
"Uniform",
lambda new_val: _gscaleCB(new_val),
slider_opts,
)
self._wgscale.setSizeClass(kw.SizeClass.MEDIUM)
self._wgscale.setTooltip("Uniform scaling factor")
# -------------
def _xscaleCB(new_val):
self.scale_xyz[0] = new_val
_updateCB()
self._wxscale = kw.Slider(
router,
"X",
lambda new_val: _xscaleCB(new_val),
slider_opts,
)
self._wxscale.setSizeClass(kw.SizeClass.MEDIUM)
self._wxscale.setTooltip("Intrinsic X axis scaling factor")
def _yscaleCB(new_val):
self.scale_xyz[1] = new_val
_updateCB()
self._wyscale = kw.Slider(
router,
"Y",
lambda new_val: _yscaleCB(new_val),
slider_opts,
)
self._wyscale.setSizeClass(kw.SizeClass.MEDIUM)
self._wyscale.setTooltip("Intrinsic Y axis scaling factor")
def _zscaleCB(new_val):
self.scale_xyz[2] = new_val
_updateCB()
self._wzscale = kw.Slider(
router,
"Z",
lambda new_val: _zscaleCB(new_val),
slider_opts,
)
self._wzscale.setSizeClass(kw.SizeClass.MEDIUM)
self._wzscale.setTooltip("Intrinsic Z axis scaling factor")
self._wlayout_iscale = widgetArray(
router,
label="Intrinsic",
children=[self._wxscale, self._wyscale, self._wzscale],
kind="inputgroup",
)
self._wlayout_scale = widgetArray(
router,
label="Scaling",
children=[self._wgscale, self._wlayout_iscale],
kind="inputgroup",
alignment="column",
alignItems="left",
)
self._wlayout_scale.setTooltip("Uniform and intrinsic scaling for the scene part")
# ------------------
self._whomtran = HomTranWidgets(
title, lambda hT: cb(km.SimTran(hT, self.gscale), self.scale_xyz), gui_context
)
# self._whomtran._wlayout_quat.addChild(self._wgscale)
self._wlayout_simtran = self._whomtran._wlayout_homtran
# self._wlayout_simtran.addChild(self._wgscale)
self._wlayout_simtran.addChild(self._wlayout_scale)
# def getTransform(self):
# return km.SimTran(self._whomtran.getTransform(), self.scale_xyz)
def setup(self, T: km.SimTran, iscale):
self._whomtran.setup(T.getTransform())
self._wgscale.setValue(T.getScale())
self._wxscale.setValue(iscale[0])
self._wyscale.setValue(iscale[1])
self._wzscale.setValue(iscale[2])
@register
class FramePairHingeCard(AbstractCard[kd.FramePairHinge]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "FramePairHinge"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ---------------------------
self._frame_pair_widgets = FramePairHingeWidgets(
self.context, group_tag=f"{self.wroot.domId()}-change_coord-group"
)
self.wroot.addChild(self._frame_pair_widgets._wik_layout)
self._frame_pair_widgets._wik_layout.setOpen(True)
"""
# --------------------------------------
# create subhinge and coordinate entries
# set IK mode on/off for coordinates
self._coord_ik = False
def _toggleCoordIK(cstate):
self._coord_ik = cstate
self._wcoord_ik = kw.Toggle(
router,
text="IK mode",
on_toggle=_toggleCoordIK,
tooltip="Toggle constraint IK mode for the slider coordinates mode",
render_as_button=True,
)
# self._wmd_coord_ik = kw.Markdown(router, text="**Articulate**", in_line=True)
# markdown to show status from IK
self._wmd_coord_ik = kw.Markdown(router, text="**IK status:**")
# slider to scale the stick parts
slider_opts = kw.SliderOptions()
slider_opts.min = -3.2
slider_opts.max = 3.2
slider_opts.step = 0.01
slider_opts.tooltip = "Scale the stick part's size by this factor"
# create sliders for max of 6 subhinges, with each having max 3
# coords. we only make visible the ones that are applicable for
# a body
self._wcoord_sliders = [None] * 18 # array with all the sliders
for shindex in range(6):
for cindex in range(3):
slider = self._wcoord_sliders[shindex * 3 + cindex] = kw.Slider(
router,
# text=f'<span style="color:#FF5733">Subhinge {shindex}/Coord {cindex}</span>',
text=f"Subhinge {shindex}/Coord {cindex}",
# need to use the shindes=shindex etc syntax to
# avoid the variable itself beig capture by the
# lambda (else only get the last value)
on_change=lambda Q, shindex=shindex, cindex=cindex: changeSubhingeCoord(
self.item,
self.context.multibody,
self.context.scene,
Q,
shindex,
cindex,
self._coord_ik,
self._wmd_coord_ik,
),
opts=slider_opts,
# tooltip="Change the scaling of the stick parts parts for the body in 3D graphics"
)
slider.setValue(0)
self._wlayout_coord_ik = kw.Layout(
router, style={"display": "flex", "flexDirection": "column", "gap": "0.1em"}
)
self.wroot.addChild(self._wlayout_coord_ik)
self._wlayout_coord_ik.addChild(self._wcoord_ik)
self._wlayout_coord_ik.addChild(self._wmd_coord_ik)
for shindex in range(6):
for cindex in range(3):
self._wlayout_coord_ik.addChild(self._wcoord_sliders[shindex * 3 + cindex])
"""
def setup(self, item: kd.FramePairHinge, item_context: kw.Json, /):
super().setup(item, item_context)
"""
_highlightFrames(
[item.oframe(), item.pframe()], toggle_over_set=False, gui_context=self.context
)
"""
self._frame_pair_widgets.setup(kc.CppWeakRef(item), kc.CppWeakRef(self.context.multibody))
# has_constraints = len(self.context.multibody.enabledConstraints()) > 0
"""
# enable subhinge/coord sliders for the coords available for the body
for shindex in range(6):
for cindex in range(3):
if shindex < item.nSubhinges() and cindex < item.subhinge(shindex).nQ():
slider = self._wcoord_sliders[shindex * 3 + cindex]
slider.setVisible(True)
slider.setValue(item.subhinge(shindex).getQ()[cindex])
else:
self._wcoord_sliders[shindex * 3 + cindex].setVisible(False)
# print("JJJJ", shindex, cindex)
"""
@register
class CompoundHingeCard(AbstractCard[kd.CompoundHinge]):
"""Card to display info about a Compound hinge."""
@property
def label(self) -> str:
return "CompoundHinge"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ---------------------------
self._cmp_hge_widgets = CompoundHingeWidgets(self.context)
self.wroot.addChild(self._cmp_hge_widgets._wik_layout)
self._cmp_hge_widgets._wik_layout.setOpen(True)
def setup(self, item: kd.CompoundHinge, item_context: kw.Json, /):
super().setup(item, item_context)
self._cmp_hge_widgets.setup(kc.CppWeakRef(item))
@register
class LoopConstraintCutJointCard(
LoopConstraintBaseCard
): # AbstractCard[kd.LoopConstraintCutJoint]):
"""Card to display info about a cut joint constraint."""
@property
def label(self) -> str:
return "LoopConstraintCutJoint"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wlcj_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wlcj_header)
self._wlcj_header1 = kw.Markdown(router, "**Node**")
self.wroot.addChild(self._wlcj_header1)
# ---------------------------
self._frame_pair_widgets = FramePairHingeWidgets(
self.context, group_tag=f"{self.wroot.domId()}-change_coord-group"
)
self.wroot.addChild(self._frame_pair_widgets._wik_layout)
self._frame_pair_widgets._wik_layout.setOpen(True)
# ---------------------------
def _tohingeCB():
self.context.selection.set(kw.Selection().dump())
snd = self.item.sourceNode()
if snd is None:
raise ValueError(f"Cannot get source node for {self.item.name()}")
sbd = snd.parentBody()
if sbd.parentHinge().hingeType() == kd.HingeType.FULL6DOF:
reverse = True
else:
tnd = self.item.targetNode()
if tnd is None:
raise ValueError(f"Cannot get target node for {self.item.name()}")
tbd = tnd.parentBody()
if tbd.parentHinge().hingeType() == kd.HingeType.FULL6DOF:
reverse = False
else:
raise ValueError(
f"One of the 2 ends of {self.item.name()} has to be 6 dof, should never get here. Cannot get target node for {self.item.name()}"
)
hinge = kd.LoopConstraintCutJoint.toPhysicalHinge(self.item, reverse)
self.context.multibody.ensureHealthy()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self.context.selection.set(kw.Selection([kw.Selection.Item(id=hinge.id())]).dump())
self._wlcj_tohinge = kw.Button(
router,
text="To hinge",
on_press=lambda: _tohingeCB(),
tooltip="Convert the cut-joint constraint into a physical hinge",
)
self._wlcj_layout_tohinge = widgetArray(
router,
label="Convert the cut-joint into a hinge",
children=[self._wlcj_tohinge],
kind="accordion", # "layout",
)
self._wlcj_layout_tohinge.setTooltip(
"Convert this cut-joint loop constraint into a physical hinge if the necessary conditions to preserve a tree topology are met."
)
self._wlcj_layout_tohinge.addChild(self._wlcj_tohinge)
self.wroot.addChild(self._wlcj_layout_tohinge)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
return isinstance(item, kd.LoopConstraintCutJoint)
def getSummary(self) -> str:
item = self.item
result = super().getSummary()
is_cutjoint = item.type() == kd.BilateralConstraintType.CUTJOINT_LOOP
if is_cutjoint:
hge_type = kd.HingeBase.hingeTypeString(item.hinge().hingeType())
return result + f"Hinge: {hge_type}"
else:
return result + "Not a cut joint"
def setup(self, item: kd.LoopConstraintCutJoint, item_context: kw.Json, /):
# set the context subtree
if isinstance(item_context, dict):
# set the context subtree
st_id = item_context.get("subtree_id", None)
if not isinstance(st_id, int):
raise ValueError("Could not get the subtree ID.")
self._subtree = cast(kd.SubTree, kc.BaseContainer.singleton().at(st_id))
if not isinstance(self._subtree, kd.SubTree):
raise ValueError(f"Did not get a SubTree from ID {st_id}.")
else:
kc.warn(
f"The subtree value is missing when selecting the {item.name()} constraint. Defaulting to multibody."
)
self._subtree = self.context.multibody
# highlight the constraint nodes and bodies involved
super().setup(item, item_context)
self._frame_pair_widgets.setup(
kc.CppWeakRef(item.hinge()), kc.CppWeakRef(self.context.multibody)
)
# can the cutjoint be converted into a physical hinge?
is_convertable_to_hinge = False
snd = item.sourceNode()
if snd:
sbd = snd.parentBody()
if sbd.parentHinge().hingeType() == kd.HingeType.FULL6DOF:
is_convertable_to_hinge = True
else:
tnd = self.item.targetNode()
if tnd:
tbd = tnd.parentBody()
if tbd.parentHinge().hingeType() == kd.HingeType.FULL6DOF:
is_convertable_to_hinge = True
self._wlcj_tohinge.setEnabled(is_convertable_to_hinge)
@register
class LoopConstraintConVelCard(LoopConstraintBaseCard): # AbstractCard[kd.LoopConstraintConVel]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "LoopConstraintConVel"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wlcc_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wlcc_header)
self._wlcc_header1 = kw.Markdown(router, "**LoopConstraintConVel**")
self.wroot.addChild(self._wlcc_header1)
# ---------------------------
# create slider for the constraint U coordinate
# markdown to show status from IK
self._wlcc_md_coord_ik = kw.Markdown(router, text="**IK status:**")
slider_opts = kw.SliderOptions()
slider_opts.min = -3.2
slider_opts.max = 3.2
slider_opts.step = 0.01
self.wroot.addChild(self._wlcc_md_coord_ik)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
return isinstance(item, kd.LoopConstraintConVel)
def setup(self, item: kd.LoopConstraintConVel, item_context: kw.Json, /):
# set the context subtree
if isinstance(item_context, dict):
# set the context subtree
st_id = item_context.get("subtree_id", None)
if not isinstance(st_id, int):
raise ValueError("Could not get the subtree ID.")
self._subtree = cast(kd.SubTree, kc.BaseContainer.singleton().at(st_id))
if not isinstance(self._subtree, kd.SubTree):
raise ValueError(f"Did not get a SubTree from ID {st_id}.")
else:
kc.warn(
f"The subtree value is missing when selecting the {item.name()} constraint. Defaulting to multibody."
)
self._subtree = self.context.multibody
super().setup(item, item_context)
# disable until we have a way to do this for convels
self._wlcc_md_coord_ik.setVisible(False)
@register
class CoordinateConstraintCard(
BilateralConstraintBaseCard
): # AbstractCard[kd.CoordinateConstraint]):
"""Card to display info about any Node object."""
@property
def label(self) -> str:
return "CoordinateConstraint"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wcc_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wcc_header)
self._wcc_header1 = kw.Markdown(router, "**CoordinateConstraint**")
self.wroot.addChild(self._wcc_header1)
# ---------------------------
# create slider for the constraint Q coordinate
# markdown to show status from IK
self._wcc_md_coord_ik = kw.Markdown(router, text="**IK status:**")
slider_opts = kw.SliderOptions()
slider_opts.min = -3.2
slider_opts.max = 3.2
slider_opts.step = 0.01
# self._wcc_coord_sliders = [None] * 18 # array with all the sliders
# callback to change subhinge coords set by the sliders
def changeCoord(item, st, scene, Q, wstatus=None):
# print("SSS", shindex, cindex, bd)
if not item:
return
# print("TTTT", shindex, cindex, hge.nSubhinges(), item.name(), Q)
sh = item.osubhinge()
sh.setQ(Q)
# do IK
# offset = st.coordOffsets(sh).Q
st.cks().freezeCoord(sh, 0, kd.CKFrozenCoordType.Q)
err = st.cks().solveQ()
if err < 1e-10:
color = "green"
stxt = "SUCCESS"
extra = f"[Q={Q:.4}]"
else:
color = "red"
stxt = "FAILED"
extra = f"[Q={Q:.4}, err={err:.4e}]"
status = f'**IK status:** <span style="color:{color}">{stxt}</span> {extra}'
# print(" err=", err, color)
st.cks().unfreezeCoord(sh, 0, kd.CKFrozenCoordType.Q)
if wstatus:
wstatus.setText(status)
scene.update()
self._wcc_coord_slider = kw.Slider(
router,
text="Coordinate",
on_change=lambda Q: changeCoord(
self.item,
self.context.multibody,
self.context.scene,
Q,
self._wcc_md_coord_ik,
),
opts=slider_opts,
# tooltip="Change the scaling of the stick parts parts for the body in 3D graphics"
)
self._wcc_coord_slider.setValue(0)
self.wroot.addChild(self._wcc_md_coord_ik)
self.wroot.addChild(self._wcc_coord_slider)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
return isinstance(item, kd.CoordinateConstraint)
def setup(self, item: kd.CoordinateConstraint, item_context: kw.Json, /):
# set the context subtree
if isinstance(item_context, dict):
# set the context subtree
st_id = item_context.get("subtree_id", None)
if not isinstance(st_id, int):
raise ValueError("Could not get the subtree ID.")
self._subtree = cast(kd.SubTree, kc.BaseContainer.singleton().at(st_id))
if not isinstance(self._subtree, kd.SubTree):
raise ValueError(f"Did not get a SubTree from ID {st_id}.")
else:
kc.warn(
f"The subtree value is missing when selecting the {item.name()} constraint. Defaulting to multibody."
)
self._subtree = self.context.multibody
super().setup(item, item_context)
@register
class SubTreeCard(AbstractCard[kd.SubTree]):
"""Card to display info about any SubTree object."""
@property
def label(self) -> str:
return "SubTree"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
# self._wst_sgheader = kw.Markdown(router, "---")
# self.wroot.addChild(self._wst_sgheader)
self._wst_header1 = kw.Markdown(router, "**SubTree**")
self.wroot.addChild(self._wst_header1)
# ----------------------------
# articulate all the bodies sequentially (only WebScene)
self._wst_swing = kw.Button(
router,
text="Sequentially articulate (free)",
on_press=lambda: _swingHinge(
self.item,
[x.parentHinge() for x in self.item.sortedBodiesList()],
True,
self.context,
),
tooltip="Sequentially swing all the coordinates in this subtree (while ignroing constraints)",
)
# self._wst_md_articulate = kw.Markdown(router, text="**Articulate**", in_line=True)
self._wst_layout_articulate = widgetArray(
router,
label="**Articulate**",
children=[self._wst_swing],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wst_layout_articulate)
# ----------------------------
self._wst_mesh = kw.Toggle(
router,
text="Mesh",
on_toggle=lambda cstate: _toggleVisibleBodies(
cstate,
self.item.sortedPhysicalBodiesList(),
self.context,
layers=ks.LAYER_PHYSICAL_GRAPHICS,
),
tooltip="Toggle the visibility of all the graphics scene parts in this subtree",
render_as_button=True,
)
# turn on/off stick parts for just the bodies in the subtree
self._wst_collision = kw.Toggle(
router,
text="Collision",
on_toggle=lambda cstate: _toggleVisibleBodies(
cstate,
self.item.sortedPhysicalBodiesList(),
self.context,
layers=ks.LAYER_COLLISION,
),
tooltip="Toggle the visibility of all the collison scene parts in this subtree",
render_as_button=True,
)
# turn on/off stick parts for just the bodies in the subtree
def stickCB(cstate, st):
has_stick = len(self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0
if not has_stick:
if not kc.allReady():
print("WARNING: kc.allReady() is failing - call mb.resetData()")
return
# assert kc.allReady()
st.multibody().createStickParts()
_toggleVisibleBodies(
cstate,
st.sortedPhysicalBodiesList(),
self.context,
layers=ks.LAYER_STICK_FIGURE,
)
# turn on/off stick parts for just the bodies in the subtree
self._wst_stick = kw.Toggle(
router,
text="Stick parts",
on_toggle=lambda cstate: stickCB(cstate, self.item),
tooltip="Toggle the visibility of all the stick parts in this subtree",
render_as_button=True,
)
# turn on/off stick parts for just the bodies in the subtree
self._wst_stick_recreate = kw.Button(
router,
text="Recreate stick parts",
on_press=lambda: recreateStickParts(self.item, self.context),
tooltip="Recreate the stick parts in this subtree",
)
# toggle wire frame/transparent view (only webscene)
self._wst_wireframe = kw.Button(
router,
text="WireFrame (TBD)",
on_press=lambda: _wireframeBodies(
[x for x in self.item.sortedBodiesList() if isinstance(x, kd.PhysicalBody)]
),
tooltip="Toggle wireframe mode for all the bodies in the subtree",
)
# self._wst_md_highlight = kw.Markdown(router, text="**Highlight**", in_line=True)
self._wst_layout_highlight = widgetArray(
router,
label="**Highlight**",
children=[
self._wst_mesh,
self._wst_collision,
self._wst_stick,
self._wst_stick_recreate,
self._wst_wireframe,
],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wst_layout_highlight)
# ----------------------------
# slider to scale the stick parts
slider_opts = kw.SliderOptions()
slider_opts.min = 0.01
slider_opts.max = 3
slider_opts.step = 0.01
self._wst_scale_stick = kw.Slider(
router,
text="Scale stick",
on_change=lambda scale: self.item.scaleStickParts(scale),
opts=slider_opts,
)
self._wst_scale_stick.setValue(1)
self.wroot.addChild(self._wst_scale_stick)
# ----------------------------
# create visjs graph
self._wst_visjs = kw.Button(
router,
text="Visjs Graph",
on_press=lambda: _createSubTreeVisJs(self.item, self.context, False),
tooltip="Create a visjs graph for the subtree (with names as labels)",
)
self._wst_visjs_ids = kw.Button(
router,
text="Visjs Graph (IDs)",
on_press=lambda: _createSubTreeVisJs(self.item, self.context, True),
tooltip="Create a visjs graph for the subtree (with ids as labels)",
)
# dictionary with all subtree treeviews created
# self._treeview = {}
# create treeview for the subtree
self._wst_treeview = kw.Button(
router,
text="TreeView",
on_press=lambda: _createSubTreeView(self.item, self.context),
tooltip="Create and add TreeView for the subtree",
)
self._wst_layout_views = widgetArray(
router,
label="**Create views**", # self._wst_md_views,
children=[
self._wst_visjs,
self._wst_visjs_ids,
self._wst_treeview,
],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wst_layout_views)
# ----------------------------
self._wst_cmframe = kw.Toggle(
router,
text="CM frame",
on_toggle=lambda cstate: _highlightFrames(
cstate,
[self.item.cmFrame()], # toggle_over_set=True,
gui_context=self.context,
),
tooltip="Enable the axes for the center of mass frame for the subtree",
render_as_button=True,
)
# show node frames
self._wst_nodes = kw.Toggle(
router,
text="Nodes",
on_toggle=lambda cstate: _highlightBodyNodes(
cstate, bodies=self.item.sortedPhysicalBodiesList(), effects=self.context.effects
),
tooltip="Toggle the axes for all the regular nodes in the subtree",
# on_press=lambda: _showBodyNodes(self.item.sortedBodiesList()),
render_as_button=True,
)
# self._wst_md_frames = kw.Markdown(router, text="**Frames**", in_line=True)
self._wst_layout_frames = widgetArray(
router,
label="**Frames**",
children=[self._wst_cmframe, self._wst_nodes],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wst_layout_frames)
# ----------------------------
self._wst_model = kw.Button(
router,
text="Model",
on_press=lambda: self.item.displayModel(),
tooltip="Run displayModel() to produce text display about the SubTree's bodies",
)
self._wst_tree = kw.Button(
router,
text="Tree",
on_press=lambda: self.item.dumpTree(),
tooltip="Run dumpModel() to produce text display about the SubTree's body topology",
)
def _dumpStateCB():
if isinstance(self.item, kd.SubGraph):
self.item.graphCoordData().dumpState()
else:
self.item.treeCoordData().dumpState()
self._wst_state = kw.Button(
router,
text="State",
on_press=lambda: _dumpStateCB(),
tooltip="Run dumpModel() to produce text display about the SubTree's body topology",
)
# self._wst_md_info = kw.Markdown(router, text="**Info**", in_line=True)
self._wst_layout_info = widgetArray(
router,
label="**Info (on terminal)**", # self._wst_md_info,
children=[self._wst_model, self._wst_tree, self._wst_state],
kind="layout", # "inputgroup",
)
# self.wroot.addChild(self._wst_layout_info)
self._wst_layout_explore = widgetArray(
router,
label="Introspect the sub-tree structure and content",
children=[
self._wst_layout_articulate, # articulate bodies
self._wst_layout_highlight, # hightlight bodies
self._wst_layout_frames, # show CM frame, nodes
self._wst_layout_views, # create visjs graphs, treeviews
self._wst_layout_info, # dump tree, display model
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wst_layout_explore.setTooltip(
"Explore the sub-tree by highlighting and articulating bodies, dumping etc"
)
self._wst_layout_explore.setOpen(True)
self.wroot.addChild(self._wst_layout_explore)
# ----------------------------
# compute forward dynamics
self._wst_fwddyn = kw.Button(
router,
text="Fwd dynamics",
on_press=lambda: kd.Algorithms.evalForwardDynamics(self.item),
tooltip="Evaluate forward dynamics for this subtree",
)
def _toggleAlgorithm(cstate: bool, st: kd.SubTree):
if cstate:
st.enableAlgorithmicUse()
else:
st.disableAlgorithmicUse()
self._wst_algthmuse = kw.Toggle(
router,
text="Enable algorithmic",
on_toggle=lambda cstate: _toggleAlgorithm(cstate, self.item),
tooltip="Enable/disable algorithmic use for the subtree",
render_as_button=True,
)
def _toggleDumpDynamics(cstate: bool, st):
# cstate = st.enable_dump_dynamics
# if not cstate:
# kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE)
# else:
# kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.WARN)
st.enable_dump_dynamics = not cstate
self._wst_debug = kw.Toggle(
router,
text="Enable dump dynamics",
on_toggle=lambda cstate: _toggleDumpDynamics(cstate, self.item),
tooltip="Toggle the 'dump dynamics' debug mode for the subtree",
render_as_button=True,
)
self._wst_layout_algthm = widgetArray(
router,
label="Forward dynamics",
children=[self._wst_algthmuse, self._wst_debug, self._wst_fwddyn],
kind="accordion", # "inputgroup",
)
self._wst_layout_algthm.setTooltip("Run forward dynamics and examine the results")
self.wroot.addChild(self._wst_layout_algthm)
# ----------------------------
# dynamics simulation
self._dynsim_widgets = DynamicsSimWidgets(self.context)
self.wroot.addChild(self._dynsim_widgets._wlayout_dynsim)
# ----------------------------
self._stdsfile = "subtreeModel.hdf5"
def _setSTModelFile(x):
self._stdsfile = x
self._wst_dsfile = kw.StringInput(
router,
"Filename",
"The model filename",
lambda x: _setSTModelFile(x),
rapid_submit=True,
)
self._wst_dsfile.setValue(self._stdsfile)
# self._wst_dsfile.setSizeClass(kw.SizeClass.MEDIUM)
self._stdsprefix = "new_"
def _setSTPrefix(x):
self._stdsprefix = x
self._wst_dsprefix = kw.StringInput(
router,
"Prefix",
"The prefix to apply to the names of the new bodies created from loading the DS file",
lambda x: _setSTPrefix(x),
rapid_submit=True,
)
self._wst_dsprefix.setValue(self._stdsprefix)
self._wst_dsprefix.setSizeClass(kw.SizeClass.NARROW)
def _toSTDSCB():
from Karana.Dynamics.SOADyn_types import SubTreeDS
ds = SubTreeDS.fromSubTree(self.item)
ds.toFile(self._stdsfile)
self._wst_saveds = kw.Button(
router,
text="Save",
on_press=lambda: _toSTDSCB(),
tooltip="Save SubTree model to file",
)
def _fromSTDSCB():
from Karana.Dynamics.SOADyn_types import SubTreeDS
ds = SubTreeDS.fromFile(self._stdsfile)
if self._stdsprefix:
ds.renameBodies(self._stdsprefix)
ds.toSubTree(self.context.multibody, self.context.multibody.virtualRoot())
_createSubTreeVisJs(self.context.multibody, self.context)
self._wst_loadds = kw.Button(
router,
text="Load",
on_press=lambda: _fromSTDSCB(),
tooltip="Load SubTree model from file (adds bodies)",
)
self._wst_layout_model = widgetArray(
router,
label="**Model data**", # self._wst_md_views,
children=[
self._wst_saveds,
self._wst_dsfile,
self._wst_loadds,
self._wst_dsprefix,
],
kind="layout", # "inputgroup",
)
# -----------------------
self._stdsinitfile = "subgraphState.hdf5"
def _setSTStateFile(x):
self._stdsinitfile = x
self._wst_dsinitfile = kw.StringInput(
router,
"State filename",
"The input/output state filename",
lambda x: _setSTStateFile(x),
rapid_submit=True,
)
self._wst_dsinitfile.setValue(self._stdsinitfile)
def _toSTDSInitCB():
from Karana.Dynamics.SOADyn_types import SubTreeStateDS
ds = SubTreeStateDS.fromSubTree(self.item)
ds.toFile(self._stdsinitfile)
self._wst_savedsinit = kw.Button(
router,
text="Save",
on_press=lambda: _toSTDSInitCB(),
tooltip="Save SubTree state to file",
)
def _fromSTDSInitCB():
from Karana.Dynamics.SOADyn_types import SubTreeStateDS
ds = SubTreeStateDS.fromFile(self._stdsinitfile)
ds.toSubTree(self.item)
self._wst_loaddsinit = kw.Button(
router,
text="Load",
on_press=lambda: _fromSTDSInitCB(),
tooltip="Load SubTree state from file",
)
self._wst_layout_state = widgetArray(
router,
label="**State data**", # self._wst_md_views,
children=[
self._wst_savedsinit,
self._wst_dsinitfile,
self._wst_loaddsinit,
],
)
self._wst_layout_ds = widgetArray(
router,
label="SubTree model and state data to/from file", # self._wst_md_views,
children=[self._wst_layout_model, self._wst_layout_state],
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wst_layout_ds)
self._wst_layout_ds.setTooltip(
"Save sub-tree model to file, and add a sub-tree of bodies from file. Also save and load state dato to and from a file"
)
# -----------------------------------------------------
# ------------------
# create a subtree clone
def _cloneCB():
st = self.item.cloneSubTree(f"{self.item.name()}_stclone")
_createSubTreeVisJs(st, self.context)
self.context.subtrees_tree_view.refresh()
self._wst_clone = kw.Button(
router,
text="Clone SubTree",
on_press=lambda: _cloneCB(),
tooltip="Create a clone of the subtree",
)
# clone the bodies in the subtree
def _cloneBodiesCB():
from Karana.Dynamics.SOADyn_types import SubTreehDS
ds = SubTreeDS.fromSubTree(self.item)
new_st = ds.toSubTree(self.context.multibody, self.context.multibody.virtualRoot())
new_st.resetData()
_createSubTreeVisJs(self.context.multibody, self.context)
self.context.mbody_tree_view.refresh()
self._wst_clone_bodies = kw.Button(
router,
text="Clone Bodies",
on_press=lambda: _cloneBodiesCB(),
tooltip="Create all the physical bodies in the subtree",
)
# discard the subtree
def _discardCB():
if self.context.multibody == self.item:
print("WARNING: cannot discard multibody system")
return
# psg = self.item.parentSubTree()
sg = self.item
self.context.selection.set(kw.Selection().dump())
kc.discard(sg)
"""
self.context.selection.set(
kw.Selection([kw.Selection.Item(id=self.context.multibody.id())]).dump()
)
"""
self.context.subtrees_tree_view.refresh()
self._wst_discard = kw.Button(
router,
text="Discard",
on_press=lambda: _discardCB(),
tooltip="Discard the subtree/subgraph (requires that there not be any other dependencies)",
)
self._wst_layout_transform = widgetArray(
router,
label="Transform the sub-tree", # self._wst_md_views,
children=[
self._wst_clone,
self._wst_clone_bodies,
self._wst_discard,
],
kind="accordion", # "inputgroup",
)
self._wst_layout_transform.setTooltip(
"Transform the system by cloning the sub-tree, its bodies or discarding it."
)
self.wroot.addChild(self._wst_layout_transform)
# ----------------------------
# change selection to one of the child subtree (drop down, or multiple buttons)
# change selection to the parent subtree
self._wst_select_down = kw.Button(
router,
text="Down",
on_press=lambda: _selectObject(_firstChildSubtree(self.item), self.context.selection),
tooltip="Change selection to a shild subtree",
)
# change selection to the parent subtree
self._wst_select_up = kw.Button(
router,
text="Up",
on_press=lambda: _selectObject(_parentSubtree(self.item), self.context.selection),
tooltip="Change selection to the parent subtree",
)
# change selection to the next sibling subtree
self._wst_select_right = kw.Button(
router,
text="Right",
on_press=lambda: _selectObject(
_siblingSubtree(self.item, True), self.context.selection
),
tooltip="Change selection to the next sibling subtree",
)
# change selection to the previous sibling subtree
self._wst_select_left = kw.Button(
router,
text="Left",
on_press=lambda: _selectObject(
_siblingSubtree(self.item, False), self.context.selection
),
tooltip="Change selection to the previous sibling subtree",
)
"""
self._wst_select_child = kw.Button(router, text="Select child (TBD)", on_press=self._doTbd)
# change selection to the parent subtree
self._wst_select_parent = kw.Button(router, text="Select parent (TBD)", on_press=self._doTbd)
# change selection to a sibling subtree
self._wst_select_sibling = kw.Button(router, text="Select sibling (TBD)", on_press=self._doTbd)
# change selection to a child body
self._wst_select_body = kw.Button(router, text="Select body (TBD)", on_press=self._doTbd)
"""
# Setup widget topology
"""
self._wst_md_visjs = kw.Markdown(router, text="**Visjs**", in_line=True)
self._wst_layout_visjs = widgetArray(router, label=self._wst_md_visjs, children=[self._wst_visjs])
self.wroot.addChild(self._wst_layout_visjs)
"""
# self._wst_md_selection = kw.Markdown(router, text="**Selection**", in_line=True)
self._wst_layout_select = widgetArray(
router,
label="Go to a related sub-tree", # self._wst_md_selection,
children=[
self._wst_select_up,
self._wst_select_left,
self._wst_select_right,
self._wst_select_down,
],
kind="accordion", # "inputgroup",
)
self._wst_layout_select.setTooltip("Change selection to other related sub-trees")
self.wroot.addChild(self._wst_layout_select)
def getSummary(self) -> str:
"""Get a markdown summary of the current item."""
# result = super().getSummary()
result = ""
st = self.item
result += f"num bodies={st.numBodies()}, num physical bodies={len(st.sortedPhysicalBodiesList())}, nQ={st.nQ()}, nU={st.nU()}</br>"
result += f"Algorithmically enabled={self.item.getVars().enabled_algorithmically()}</br>"
return result
def setup(self, item: kd.SubTree, item_context: kw.Json, /):
super().setup(item, item_context)
# hide the header is this is the most derived class
if not isinstance(item, kd.SubGraph):
# self._wst_sgheader.setVisible(False)
self._wst_header1.setVisible(False)
scene = self.context.scene
has_mesh = len(scene.getSceneParts(ks.LAYER_PHYSICAL_GRAPHICS)) > 0
has_stick = len(scene.getSceneParts(ks.LAYER_STICK_FIGURE)) > 0
has_collision = len(scene.getSceneParts(ks.LAYER_COLLISION)) > 0
parent = item.parentSubTree()
has_parent = parent is not None
has_siblings = has_parent and len(parent.childrenSubTrees()) > 1
has_children = len(item.childrenSubTrees()) > 0
has_compound_bodies = item.hasCompoundBodies()
is_algorithm_enabled = item.getVars().enabled_algorithmically()
self._wst_mesh.setVisible(has_mesh)
self._wst_mesh.setValue(has_mesh)
self._wst_clone_bodies.setVisible(not has_compound_bodies)
# self._wst_stick.setVisible(has_stick)
self._wst_stick.setValue(has_stick)
self._wst_scale_stick.setVisible(has_stick)
self._wst_collision.setVisible(has_collision)
self._wst_collision.setValue(False)
self._wst_algthmuse.setValue(is_algorithm_enabled)
self._wst_select_up.setVisible(has_parent)
self._wst_select_left.setVisible(has_parent)
self._wst_select_right.setVisible(has_parent)
self._wst_select_left.setVisible(has_siblings)
self._wst_select_right.setVisible(has_siblings)
# self._wst_md_selection.setVisible(has_children)
self._wst_select_down.setVisible(has_children)
self._wst_debug.setValue(item.enable_dump_dynamics)
self.full_Q = self.context.multibody.getQ()
self.full_U = self.context.multibody.getU()
sp = None
self._dynsim_widgets.setup(kc.CppWeakRef(item), sp)
"""
self._dyn_sp = None
self._wst_dynsim_setup.setValue(False)
self._wst_dynsim_run.setEnabled(False)
self._wst_coord_full_reset.setEnabled(False)
self._wst_enable_gravity.setEnabled(True)
self._wst_enable_contact.setEnabled(True)
"""
"""
# hightlight the subtree bodies in the multibody visjs graph
_highlightBodies(
bodies=item.sortedPhysicalBodiesList(),
secondary_bodies=[],
tertiary_bodies=[],
toggle_over_set=False,
gui_context=self.context,
)
"""
# create a visjs graph for the subtree if there is not one
if False:
gui = self.context.gui
if item.id() not in gui.visjs_servers:
# the graph does not exist, create it
_createSubTreeVisJs(item, self.context)
if 1:
# TODO - until implemented
self._wst_wireframe.setVisible(False)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
if isinstance(item, kd.SubGraph):
return False
return isinstance(item, self.wrapped())
@register
class SubGraphCard(SubTreeCard): # AbstractCard[kd.SubGraph]):
# class SubGraphCard(AbstractCard[kd.SubGraph]):
"""Card to display info about a SubGraph instance."""
@property
def label(self) -> str:
return "SubGraph"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wsg_header = kw.Markdown(router, "---")
self.wroot.addChild(self._wsg_header)
self._wsg_header1 = kw.Markdown(router, "**SubGraph**")
self.wroot.addChild(self._wsg_header1)
# ------------------------------
# articulate all the bodies sequentially with IK solver on (only WebScene)
self._wsg_swing_ik = kw.Button(
router,
text="Sequentially articulate (constrained)",
on_press=lambda: _swingHinge(
self.item,
[x.parentHinge() for x in self.item.sortedBodiesList()],
False,
self.context,
),
tooltip="Sequentially swing all the coordinates in the subgraph while enforcing bilateral constraints",
)
# ----------------------------
def _toggleHighlightConstraints(
cstate: bool,
sg: kd.SubGraph,
# bodies: list[kd.PhysicalBody],
gui_context: GuiContext,
):
"""Highlight constraints and the constraint nodes involving the list of bodies in visjs."""
# AG - this should be toggleable, so can turn off the frame axes
# gui = gui_context.gui
if sg.id() not in gui_context.visjs_servers:
# the graph does not exist, create it. It will show the
# constraint edges by default.
_createSubTreeVisJs(sg, gui_context)
return
server, _ = gui_context.visjs_servers[sg.id()]
new_state = cstate
gui_context.visjs_servers[sg.id()] = (server, new_state)
# label = "constraints" if self._constraints_state else "tree"
if new_state:
server.enableSubGraph("constraints")
else:
server.disableSubGraph("constraints")
# show constraints (for loop constraint show nodes, for coordinate perhaps a line in webscene)
# self._constraints_state = True
self._wsg_constraints = kw.Toggle(
router,
text="Graph constraint edges",
on_toggle=lambda cstate: _toggleHighlightConstraints(
cstate, sg=self.item, gui_context=self.context
),
render_as_button=True,
tooltip="Toggle the bilateral constraint edges in the visjs graph for this subgraph",
)
# self._wsg_md_articulate = kw.Markdown(router, text="**Articulate**", in_line=True)
self._wsg_layout_viz = widgetArray(
router,
label="Introspect the sub-graph structure and content",
children=[self._wsg_swing_ik, self._wsg_constraints],
kind="accordion", # "layout" # "inputgroup",
)
self._wsg_layout_viz.setTooltip(
"Explore the sub-graph by highlighting and articulating bodies with constraints "
)
self._wsg_layout_viz.setOpen(True)
self.wroot.addChild(self._wsg_layout_viz)
# self.wroot.addChild(self._wsg_constraints)
# ----------------------------
self._sgdsfile = "subgraphModel.hdf5"
def _setSGModelFile(x):
self._sgdsfile = x
self._wsg_dsfile = kw.StringInput(
router,
"Model filename",
"The input/output DataStruct filename",
lambda x: _setSGModelFile(x),
rapid_submit=True,
)
self._wsg_dsfile.setValue(self._sgdsfile)
self._sgdsprefix = "new_"
def _setSGPrefix(x):
self._sgdsprefix = x
self._wsg_dsprefix = kw.StringInput(
router,
"Prefix",
"The prefix to apply to the names of the new bodies created from loading the DS file",
lambda x: _setSGPrefix(x),
rapid_submit=True,
)
self._wsg_dsprefix.setValue(self._sgdsprefix)
self._wsg_dsprefix.setSizeClass(kw.SizeClass.NARROW)
def _toSGDSCB():
from Karana.Dynamics.SOADyn_types import SubGraphDS
ds = SubGraphDS.fromSubGraph(self.item)
ds.toFile(self._sgdsfile)
self._wsg_saveds = kw.Button(
router,
text="Save",
on_press=lambda: _toSGDSCB(),
tooltip="Save SubGraph model to file",
)
def _fromSGDSCB():
from Karana.Dynamics.SOADyn_types import SubGraphDS
ds = SubGraphDS.fromFile(self._sgdsfile)
if self._sgdsprefix:
ds.renameBodies(self._sgdsprefix)
ds.toSubGraph(self.context.multibody, self.context.multibody.virtualRoot())
_createSubTreeVisJs(self.context.multibody, self.context)
self._wsg_loadds = kw.Button(
router,
text="Load",
on_press=lambda: _fromSGDSCB(),
tooltip="Load SubGraph model from file (adds bodies and constraints)",
)
self._wsg_layout_model = widgetArray(
router,
label="**Model file**", # self._wsg_md_views,
children=[
self._wsg_saveds,
self._wsg_dsfile,
self._wsg_loadds,
self._wsg_dsprefix,
],
)
# self.wroot.addChild(self._wsg_layout_ds)
# -----------------------
self._sgdsinitfile = "subgraphState.hdf5"
def _setSGStateFile(x):
self._sgdsinitfile = x
self._wsg_dsinitfile = kw.StringInput(
router,
"State filename",
"The input/output state filename",
lambda x: _setSGStateFile(x),
rapid_submit=True,
)
self._wsg_dsinitfile.setValue(self._sgdsinitfile)
def _toSGDSInitCB():
from Karana.Dynamics.SOADyn_types import SubGraphStateDS
ds = SubGraphStateDS.fromSubGraph(self.item)
ds.toFile(self._sgdsinitfile)
self._wsg_savedsinit = kw.Button(
router,
text="Save",
on_press=lambda: _toSGDSInitCB(),
tooltip="Save SubGraph state to file",
)
def _fromSGDSInitCB():
from Karana.Dynamics.SOADyn_types import SubGraphStateDS
ds = SubGraphStateDS.fromFile(self._sgdsinitfile)
ds.toSubGraph(self.item)
self._wsg_loaddsinit = kw.Button(
router,
text="Load",
on_press=lambda: _fromSGDSInitCB(),
tooltip="Load SubGraph state from file",
)
self._wsg_layout_state = widgetArray(
router,
label="**State data**", # self._wsg_md_views,
children=[
self._wsg_savedsinit,
self._wsg_dsinitfile,
self._wsg_loaddsinit,
],
)
self._wsg_layout_ds = widgetArray(
router,
label="SubGraph model and state data to/from file", # self._wsg_md_views,
children=[self._wsg_layout_model, self._wsg_layout_state],
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wsg_layout_ds)
self._wsg_layout_ds.setTooltip(
"Save sub-graph model to file, and add a sub-graph of bodies & constraints from file. Also save and load state dato to and from a file"
)
# ------------------------------------
def _flattenCB(sg):
if not sg.hasCompoundBodies():
return
sg.flattenCompoundBodies()
# create a subgraph clone
def _cloneCB():
sg = self.item.cloneSubGraph(f"{self.item.name()}_sgclone")
_createSubTreeVisJs(sg, self.context)
self._wsg_clone = kw.Button(
router,
text="Clone SubGraph",
on_press=lambda: _cloneCB(),
tooltip="Create a clone of the subgraph",
)
# clone the bodies in the subgraph
def _cloneBodiesCB():
from Karana.Dynamics.SOADyn_types import SubGraphDS
ds = SubGraphDS.fromSubGraph(self.item)
IdMixin.resetAllObjectsFromIds()
new_sg = ds.toSubGraph(self.context.multibody, self.context.multibody.virtualRoot())
new_sg.resetData()
_createSubTreeVisJs(self.context.multibody, self.context)
self._wsg_clone_bodies = kw.Button(
router,
text="Clone Bodies",
on_press=lambda: _cloneBodiesCB(),
tooltip="Create all the physical bodies and constraints in the subgraph",
)
# highlight bodies in aggregation graph for a constraint
self._wsg_flatten = kw.Button(
router,
text="Flatten compound bodies",
on_press=lambda: _flattenCB(self.item),
tooltip="Flatten all nested compound bodies in the subgraph",
)
# self._wsg_md_flatten = kw.Markdown(router, text="**Flatten**", in_line=True)
self._wsg_layout_transform = widgetArray(
router,
label="Transform the sub-graph",
children=[self._wsg_clone, self._wsg_clone_bodies, self._wsg_flatten],
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wsg_layout_transform)
self._wsg_layout_transform.setTooltip(
"Transform the system by cloning a sub-graph, its bodies or flattening its compound bodies."
)
# -------------------
def updateBaumDamping(damping) -> None:
self.item.setBaumgarteDamping(damping)
def updateBaumStiffness(stiffness) -> None:
self.item.setBaumgarteDamping(stiffness)
slider_opts = kw.SliderOptions()
slider_opts.min = -4
slider_opts.max = 4
slider_opts.log_scale = True
self._wsg_baum_damping = kw.Slider(
router, text="Damping", on_change=updateBaumDamping, opts=slider_opts
)
self._wsg_baum_stiffness = kw.Slider(
router, text="Stiffness", on_change=updateBaumStiffness, opts=slider_opts
)
self._wsg_baum_damping.setSizeClass(kw.SizeClass.MEDIUM)
self._wsg_baum_stiffness.setSizeClass(kw.SizeClass.MEDIUM)
self._wsg_baum_sliders = widgetArray(
router, [self._wsg_baum_damping, self._wsg_baum_stiffness], alignment="row"
)
def toggleBaum(enabled):
self.item.setBaumgarteEnabled(enabled)
self._wsg_baum_sliders.setEnabled(enabled)
self._wsg_baum_enable = kw.Toggle(router, "Enable Baumgarte Stabilization", toggleBaum)
self._wsg_baum_group = widgetArray(
router,
label="Baumgarte Stabilization",
kind="accordion", # "inputgroup",
children=[self._wsg_baum_enable, self._wsg_baum_sliders],
alignment="column",
alignItems="left",
)
self._wsg_baum_group.setTooltip("Enabled Baumgarte stabilization for the dynamics")
self.wroot.addChild(self._wsg_baum_group)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
if isinstance(item, kd.Multibody):
result = False
else:
result = isinstance(item, kd.SubGraph) # self.wrapped())
# print(" BBB", item, self.wrapped(), result)
return result
def setup(self, item: kd.SubGraph, item_context: kw.Json, /):
super().setup(item, item_context)
has_constraints = len(item.enabledConstraints()) > 0
has_compound_bodies = item.hasCompoundBodies()
# Setup baumgarte settings
self._wsg_baum_enable.setValue(item.getBaumgarteEnabled())
self._wsg_baum_sliders.setEnabled(item.getBaumgarteEnabled())
if not km.isNotReadyNaN(item.getBaumgarteDamping()):
self._wsg_baum_damping.setValue(item.getBaumgarteDamping())
if not km.isNotReadyNaN(item.getBaumgarteStiffness()):
self._wsg_baum_stiffness.setValue(item.getBaumgarteStiffness())
# self._wsg_md_articulate.setVisible(has_constraints)
self._wsg_swing_ik.setEnabled(has_constraints)
# self._wsg_constraints.setVisible(has_constraints)
self._wsg_constraints.setEnabled(has_constraints)
if False:
# create a visjs graph for the subgraph if there is not one
gui = self.context.gui
if item.id() not in gui.visjs_servers:
# the graph does not exist, create it
_createSubTreeVisJs(item, self.context)
self._wsg_clone_bodies.setEnabled(not has_compound_bodies)
self._wsg_flatten.setEnabled(has_compound_bodies)
self._wsg_baum_group.setEnabled(has_constraints)
"""
# hightlight the subtree bodies in the multibody visjs graph
_highlightBodies(
bodies=item.sortedPhysicalBodiesList(),
secondary_bodies=[],
tertiary_bodies=[],
toggle_over_set=False,
gui_context=self.context,
)
"""
# set the default state. Skipping this for now, since it is
# triggering the creation of the graph for the subtree with the
# subtree is selected
if 0:
self._wsg_constraints.setValue(True)
@register
class MultibodyCard(SubGraphCard): # AbstractCard[kd.Multibody]):
"""Card to display info about any Multibody-derived object."""
@property
def label(self) -> str:
return "Mbody"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------------
self._wmb_sgheader = kw.Markdown(router, "---")
self.wroot.addChild(self._wmb_sgheader)
self._wmb_sgheader1 = kw.Markdown(router, "**Multibody**")
self.wroot.addChild(self._wmb_sgheader1)
# -----------------------------
# select camera mask
def _setCameraMask(cstate, mask):
graphics = self.context.graphics
camera = graphics.defaultCamera()
if not camera:
return
cmask = camera.getMask()
if cstate:
cmask |= mask
if mask == ks.LAYER_STICK_FIGURE:
if not self.context.scene.getSceneParts(ks.LAYER_STICK_FIGURE):
self.context.multibody.createStickParts()
else:
cmask ^= mask
camera.setMask(cmask)
self._wmb_camera_graphics = kw.Toggle(
router,
text="Physical",
on_toggle=lambda cstate: _setCameraMask(cstate, ks.LAYER_PHYSICAL_GRAPHICS),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE),
tooltip="Enable/disable physical graphics meshes in the 3D display",
render_as_button=True,
)
self._wmb_camera_ornamental = kw.Toggle(
router,
text="Ornamental",
on_toggle=lambda cstate: _setCameraMask(cstate, ks.LAYER_ORNAMENTAL),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE),
# tooltip="Change the 3D graphics camera mask",
tooltip="Enable/disable ornamental meshes in the 3D display",
render_as_button=True,
)
self._wmb_camera_collision = kw.Toggle(
router,
text="Collision",
on_toggle=lambda cstate: _setCameraMask(cstate, ks.LAYER_COLLISION),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE),
# tooltip="Change the 3D graphics camera mask",
tooltip="Enable/disable collision meshes in the 3D display",
render_as_button=True,
)
self._wmb_camera_stick = kw.Toggle(
router,
text="Stick",
on_toggle=lambda cstate: _setCameraMask(cstate, ks.LAYER_STICK_FIGURE),
# on_press=lambda: kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE),
# tooltip="Change the 3D graphics camera mask",
tooltip="Enable/disable stick figure meshes in the 3D display",
render_as_button=True,
)
self._wmb_layout_camera = widgetArray(
router,
label="Mesh types",
children=[
self._wmb_camera_graphics,
self._wmb_camera_ornamental,
self._wmb_camera_collision,
self._wmb_camera_stick,
],
kind="layout",
)
# ------------
# shadow parameters
self._shadows_radius = 1
self._shadows_pixels = 2048
def _shadowsUpdate():
self.context.graphics._setShadows(
np.array([0, 0, -1]), radius=self._shadows_radius, pixels=self._shadows_pixels
)
# slider to scale the stick parts
slider_opts = kw.SliderOptions()
slider_opts.min = -1
slider_opts.max = 1
slider_opts.step = 0.01
slider_opts.log_scale = True
def _shadowsRadiusCB(radius):
self._shadows_radius = radius
_shadowsUpdate()
self._wmb_shadows_radius = kw.Slider(
router,
text="Radius",
on_change=_shadowsRadiusCB,
opts=slider_opts,
)
self._wmb_shadows_radius.setValue(self._shadows_radius)
self._wmb_shadows_radius.setTooltip("The shadows radius parameter")
self._wmb_shadows_radius.setSizeClass(kw.SizeClass.MEDIUM)
def _shadowsPixelsCB(val):
self.branch_length = val
_shadowsUpdate()
self._wmb_shadows_pixels = kw.IntInput(
router,
"Pixels",
tooltip="The shadows pixel parameter",
on_change=_shadowsPixelsCB,
)
self._wmb_shadows_pixels.setValue(self._shadows_pixels)
self._wmb_shadows_pixels.setSizeClass(kw.SizeClass.MEDIUM)
self._wmb_layout_shadows = widgetArray(
router,
label="Shadows settings",
children=[
self._wmb_shadows_radius,
self._wmb_shadows_pixels,
],
kind="layout",
)
# ------------
# ---------------------------------
# slider to scale the stick parts
slider_opts = kw.SliderOptions()
slider_opts.min = 0.01
slider_opts.max = 3
slider_opts.step = 0.01
def _scaleCB(bd, scale):
if bd:
bd.scaleStickParts(scale)
self._wmb_scale_stick = kw.Slider(
router,
text="Scale stick",
on_change=lambda scale: _scaleCB(self.item, scale),
opts=slider_opts,
# tooltip="Change the scaling of the stick parts parts for the body in 3D graphics"
)
self._wmb_scale_stick.setValue(1)
self._wmb_scale_stick.setSizeClass(kw.SizeClass.MEDIUM)
# self.wroot.addChild(self._wmb_scale_stick)
# recreate/delete stick figures and scaling
def _recreateStickParts():
self.item.removeStickParts()
self.item.createStickParts()
self._wmb_stick_recreate = kw.Button(
router,
text="Recreate",
on_press=lambda: _recreateStickParts(),
tooltip="Recreate the stick parts",
)
self._wmb_stick_delete = kw.Button(
router,
text="Remove",
on_press=lambda: self.item.removeStickParts(),
tooltip="Delete and remove the stick parts",
)
self._wmb_layout_stick = widgetArray(
router,
label="Stick parts",
children=[
self._wmb_scale_stick,
self._wmb_stick_recreate,
self._wmb_stick_delete,
],
kind="layout",
)
# ----------------
# set background color - TODO
def _background(clr):
self.context.graphics._setBackgroundColor(clr)
self._wmb_bg_black = kw.Button(
router,
text="Black",
on_press=lambda: _background(ks.Color.BLACK),
tooltip="Change the background color to BLACK",
)
self._wmb_bg_skyblue = kw.Button(
router,
text="Skyblue",
on_press=lambda: _background(ks.Color.SKYBLUE),
tooltip="Change the background color to SKYBLUE",
)
self._wmb_bg_palegoldenrod = kw.Button(
router,
text="Palegoldenrod",
on_press=lambda: _background(ks.Color.PALEGOLDENROD),
tooltip="Change the background color to PALEGOLDENROD",
)
self._wmb_layout_bgclr = widgetArray(
router,
label="Background color",
children=[
self._wmb_bg_black,
self._wmb_bg_skyblue,
self._wmb_bg_palegoldenrod,
],
kind="layout",
)
# ----------------
# show the 3D pane
def _addWebScene():
self.context.dock.addChild(
title="3D View",
widget=self.context.graphics_frame,
)
self._wmb_webscene = kw.Button(
router,
text="Show 3D graphics pane",
on_press=lambda: _addWebScene(),
tooltip="Make the 3D graphics panel visible is missing",
)
self._wmb_layout_viz = widgetArray(
router,
label="Tailor 3D graphics content",
children=[
self._wmb_layout_shadows,
self._wmb_layout_camera,
self._wmb_layout_stick,
self._wmb_layout_bgclr,
self._wmb_webscene,
],
kind="accordion", # "layout",
alignment="column",
alignItems="left",
)
self._wmb_layout_viz.setTooltip("Tailor the 3D graphics content")
self._wmb_layout_viz.setOpen(True)
self.wroot.addChild(self._wmb_layout_viz)
# -----------------------------
def _createFramesGraph(use_names):
_createFramesVisJs(use_names, self.context)
self._wmb_framesgraph_ids = kw.Button(
router,
text="Id labels",
on_press=lambda: _createFramesGraph(True),
tooltip="Create the frames graph with frame ids as labels",
)
self._wmb_framesgraph = kw.Button(
router,
text="Name labels",
on_press=lambda: _createFramesGraph(False),
tooltip="Create the frames graph with frame names as labels",
)
self._wmb_layout_graphs = widgetArray(
router,
label="Create frames graph",
children=[self._wmb_framesgraph, self._wmb_framesgraph_ids],
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wmb_layout_graphs)
# ------------------------------------
def _setupCE(mb):
# first check that a cegraph exists, else create it
# nbodies = len(mb.sortedPhysicalBodiesList())
if not mb.enabledConstraints():
return
import random
suff = int(random.random() * 1000)
cegraph = kd.SubGraph.create(f"cegraph{suff}", mb, mb.virtualRoot())
cegraph.setupConstraintEmbedding()
cegraph.flattenCompoundBodies()
_createSubTreeVisJs(cegraph, self.context)
_createSubTreeView(cegraph, self.context)
# highlight bodies in aggregation graph for a constraint
self._wmb_fullce = kw.Button(
router,
text="TA to CE model ",
on_press=lambda: _setupCE(self.item),
tooltip="Create CE compound bodies for all the multibody constraints",
)
def _toFACB():
self.item.toFullyAugmentedModel()
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self._wmb_fullyaug = kw.Button(
router,
text="TA to FA model",
on_press=lambda: _toFACB(),
tooltip="Convert the tree augmented multibody model into a fully augmnented model",
)
def _toTACB():
mb = self.context.multibody
mb.ensureHealthy()
from KaranaTest.Dynamics.fa2ta import TreeConverter
fa2ta = TreeConverter(verbose=True, parent_prefs={}, skip_prefs={})
# breadth first
basebds = []
fa2ta.visited = list()
fa2ta.todo = mb.sortedPhysicalBodiesList()
while fa2ta.todo:
while fa2ta.visited:
fa2ta.convertBranchBF(basebds)
if fa2ta.todo:
fa2ta.visited = [fa2ta.todo[0]]
mb.ensureHealthy()
_createSubTreeVisJs(mb, self.context)
self._wmb_ta = kw.Button(
router,
text="FA to TA model",
on_press=lambda: _toTACB(),
tooltip="Convert the full augmented multibody model into a tree augmnented model",
)
self._wmb_layout_topol = widgetArray(
router,
label="Transform multibody topology",
children=[
self._wmb_fullyaug,
self._wmb_ta,
self._wmb_fullce,
], # self._wmb_modelscript, self._wmb_mdloutfile],
kind="accordion", # "inputgroup",
)
self._wmb_layout_topol.setTooltip(
"Transform the multibody system for constraint embedding and into a FA model"
)
self.wroot.addChild(self._wmb_layout_topol)
# -----------------------------
# add bodies
self._flex_mode = False
self._bd_prefix = "dummy"
def _setPrefixCB(val):
self._bd_prefix = val
self._wmb_bdprefix = kw.StringInput(
router,
"Prefix",
on_change=lambda val: _setPrefixCB(val),
rapid_submit=True,
)
self._wmb_bdprefix.setTooltip("The prefix to use when creating bodies")
self._wmb_bdprefix.setSizeClass(kw.SizeClass.WIDE)
self._wmb_bdprefix.setValue(self._bd_prefix)
def _flexMode(cstate):
self._flex_mode = cstate
self._wmb_flex_body = kw.Toggle(
router,
text="Enable flex bodies",
on_toggle=lambda cstate: _flexMode(cstate),
tooltip="If on, bodies fill be flexible, else rigid",
render_as_button=True,
)
mass = 1
dist = 0.2
b2j = km.HomTran(km.UnitQuaternion(0.8, 0, 0.6, 0), np.array((0, dist, 0)))
inb2j = km.HomTran(km.UnitQuaternion(0.5, 0.5, 0.5, 0.5), np.array((0, -dist, 0)))
axis = np.array((0, 0, 1.0))
body_to_cm = np.array((0.1, 0.11, 0.13))
inertia = np.array(((1, 0, 0), (0, 1, 0), (0, 0, 1)))
spI = km.SpatialInertia(mass, body_to_cm, inertia)
rigid_bdparams = kd.PhysicalBodyParams(
spI,
b2j,
inb2j,
kd.PhysicalHingeParams(
hinge_type=kd.HingeType.REVOLUTE,
subhinge_params=[kd.PinSubhingeParams(unit_axis=np.array([0.0, 0.0, 1.0]))],
),
)
htype = kd.HingeType.REVOLUTE
# TODO - Not working current
def _addSingleBody():
bdparams = kd.PhysicalBodyParams(
spI,
km.HomTran(),
inb2j,
hinge_params=kd.PhysicalHingeParams(
hinge_type=kd.HingeType.FULL6DOF,
subhinge_params=[kd.Linear3SubhingeParams(), kd.SphericalSubhingeParams()],
),
)
bds = kd.PhysicalBody.addSerialChain(
self._bd_prefix, 1, self.context.multibody.virtualRoot(), bdparams
)
self.context.multibody.ensureHealthy()
for bd in bds:
hge = bd.parentHinge()
hge.coordData().setQ(0)
hge.coordData().setU(0)
hge.coordData().setT(0)
hge.coordData().setUdot(0)
bd.setGravAccel([0, 0, 0])
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
self._wmb_onebody = kw.Button(
router,
text="Single floating body",
on_press=lambda: _addSingleBody(),
tooltip="Add a single body",
)
# add a tree of bodies
self.branch_length = 3
self.nbranches = 2 # 4
self.depth = 2 # 3
def _setBranchLength(val):
self.branch_length = val
self._wmb_branch_length = kw.IntInput(
router,
"Branch length",
"The branch length for the new serial chain/tree system",
on_change=_setBranchLength,
)
self._wmb_branch_length.setValue(self.branch_length)
self._wmb_branch_length.setSizeClass(kw.SizeClass.NARROW)
def _setNbranches(val):
self.nbranches = val
self._wmb_nbranches = kw.IntInput(
router,
"NBranches",
"The number of branches for the new tree system",
on_change=_setNbranches,
)
self._wmb_nbranches.setValue(self.nbranches)
self._wmb_nbranches.setSizeClass(kw.SizeClass.NARROW)
def _setDepth(val):
self.depth = val
self._wmb_depth = kw.IntInput(
router,
"Depth",
"The depth of the new tree system",
on_change=_setDepth,
)
self._wmb_depth.setValue(self.depth)
self._wmb_depth.setSizeClass(kw.SizeClass.NARROW)
# TODO - Not working current
def _addSerialChain():
bds = kd.PhysicalBody.addSerialChain(
self._bd_prefix,
self.branch_length,
self.context.multibody.virtualRoot(),
rigid_bdparams,
)
self.context.multibody.ensureHealthy()
for bd in bds:
hge = bd.parentHinge()
hge.coordData().setQ(0)
hge.coordData().setU(0)
hge.coordData().setT(0)
hge.coordData().setUdot(0)
bd.setGravAccel([0, 0, 0])
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
recreateStickParts(self.item, self.context)
self._wmb_serialchain = kw.Button(
router,
text="Serial chain",
on_press=lambda: _addSerialChain(),
tooltip="Add a serial chain",
)
def _addTree():
bds = kd.PhysicalBody.addTree(
self._bd_prefix,
self.branch_length,
self.nbranches,
self.depth,
self.context.multibody.virtualRoot(),
rigid_bdparams,
)
self.context.multibody.ensureHealthy()
for bd in bds:
hge = bd.parentHinge()
hge.coordData().setQ(0)
hge.coordData().setU(0)
hge.coordData().setT(0)
hge.coordData().setUdot(0)
bd.setGravAccel([0, 0, 0])
_createSubTreeVisJs(self.context.multibody, self.context)
# _createSubTreeView(self.context.multibody, self.context)
recreateStickParts(self.item, self.context)
# TODO - Not working current
self._wmb_bodiestree = kw.Button(
router,
text="Tree",
on_press=lambda: _addTree(),
tooltip="Add a tree of bodies",
)
# TODO - add way to enter number of bodies, number of branches, depth
self._wmb_layout_addbodies = widgetArray(
router,
label="Add bodies",
children=[
self._wmb_bdprefix,
self._wmb_onebody,
self._wmb_serialchain,
self._wmb_bodiestree,
],
kind="inputgroup",
)
self._wmb_layout_addprops = widgetArray(
router,
label="Serial Chain/Tree properties",
children=[
self._wmb_flex_body,
self._wmb_branch_length,
self._wmb_nbranches,
self._wmb_depth,
],
kind="inputgroup",
)
self._wmb_layout_procedural = widgetArray(
router,
label="Procedurally add physical bodies to the multibody system",
children=[
self._wmb_layout_addbodies,
self._wmb_layout_addprops,
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wmb_layout_procedural.setTooltip(
"Procedurally add single, serial chain, and sub-tree of rigid and deformable bodies to the multibody system"
)
self.wroot.addChild(self._wmb_layout_procedural)
# -----------------------------
"""
self._wmb_gravity = kw.Button(
router,
text="Show gravity vector (TBD)",
on_press=lambda: self._doTbd(),
tooltip="Toggle visualization of the gravity acceleration vector",
)
# Setup widget topology
# self._wmb_md_gravity = kw.Markdown(router, text="**Gravity**", in_line=True)
self._wmb_layout_grav = widgetArray(
router,
label="Gravity",
children=[self._wmb_gravity],
kind="inputgroup",
)
self.wroot.addChild(self._wmb_layout_grav)
"""
def _toggleStickParts(self):
graphics = self.context.graphics
camera = graphics.defaultCamera()
if not camera:
return
mask = camera.getMask()
mask ^= ks.LAYER_STICK_FIGURE
camera.setMask(mask)
def setup(self, item: kd.Multibody, item_context: kw.Json, /):
super().setup(item, item_context)
has_constraints = len(item.enabledConstraints()) > 0
self._wmb_fullce.setEnabled(has_constraints)
graphics = self.context.graphics
camera = graphics.defaultCamera()
if camera:
mask = camera.getMask()
self._wmb_camera_graphics.setValue(mask & ks.LAYER_PHYSICAL_GRAPHICS)
self._wmb_camera_ornamental.setValue(mask & ks.LAYER_ORNAMENTAL)
self._wmb_camera_collision.setValue(mask & ks.LAYER_COLLISION)
self._wmb_camera_stick.setValue(mask & ks.LAYER_STICK_FIGURE)
def isCompatible(self, item: kd.Any) -> bool:
"""Check whether the Card knows how to display an item."""
result = isinstance(item, kd.Multibody) # self.wrapped())
# print(" CCC Multibody", item, self.wrapped(), result)
return result
@register
class ModelManagerCard(AbstractCard[kd.ModelManager]):
"""Card to display info about any ModelManager-derived object."""
@property
def label(self) -> str:
return "ModelManager"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
def _toggleTrace(cstate):
mm = self.item
# Might happen if the toggle state gets set before we are ready
if mm is None:
return
if not cstate:
kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.TRACE)
else:
kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.WARN)
mm.trace_state_propagator = cstate
self._wtrace = kw.Toggle(
router,
text="Trace mode",
on_toggle=_toggleTrace,
render_as_button=True,
tooltip="Enable/disable trace mode for the model manager to display mode related information",
)
# Setup widget topology
# self._wmd_info = kw.Markdown(router, text="**Info**")
self._wlayout_info = widgetArray(
router,
label="Introspect",
children=[self._wtrace],
kind="inputgroup",
)
self.wroot.addChild(self._wlayout_info)
def setup(self, item: kd.ModelManager, item_context: kw.Json, /):
super().setup(item, item_context)
def createPauseCb(sp: kd.StatePropagator) -> Callable[[], None]:
"""Return the callback for a pause/resume button.
Parameters
----------
sp : kd.StatePropagator
The StatePropagator to pause/resume.
Returns
-------
Callable[[], None]
The pause/resume callback.
"""
assert sp
weakref = kc.CppWeakRef(sp)
def inner():
sp = weakref()
if sp:
if sp.isPaused():
sp.resume()
else:
sp.pause()
else:
kc.error("Callback called after StatePropagator was destroyed.")
return inner
def createAdvanceByCallback(
sp: kd.StatePropagator | None,
gui_context: GuiContext,
time: np.timedelta64,
wstatus: kw.Markdown,
):
"""Callback for a pause/resume button.
Parameters
----------
sp : kd.StatePropagator
The StatePropagator to pause/resume.
gui_context: GuiContext
Handle to various GUI helpers
time : np.timedelta64
The time to advance by.
wstatus : kw.Markdown
The markdown widget to display the status.
"""
assert sp
async def task():
color = "red"
stxt = "Running ..."
status = f'**Status:** <span style="color:{color}">{stxt}</span>'
wstatus.setText(status)
if sp.isPaused():
pause_at = time + sp.getTimeKeeper().getTime()
te = kd.TimedEvent(f"pause_at_{pause_at}", pause_at, lambda _: sp.pause(), False)
sp.registerTimedEvent(te)
sp.resume()
else:
sp.advanceBy(time)
"""
for _ in range(10):
print(f"Advancing to {sp.getTime()} ...")
sp.advanceBy(time / 10)
"""
color = "green"
stxt = "Idle"
status = f'**Status:** <span style="color:{color}">{stxt}</span>'
wstatus.setText(status)
# Run the task on a background thread to avoid blocking the GUI thread for a long time
gui_context.worker.run(task)
@register
class StatePropagatorCard(AbstractCard[kd.StatePropagator]):
"""Card to display info about any StatePropagator-derived object."""
@property
def label(self) -> str:
return "StatePropagator"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ----------------------------
# dynamics simulation
self._dynsim_widgets = DynamicsSimWidgets(self.context)
self.wroot.addChild(self._dynsim_widgets._wlayout_dynsim)
self._dynsim_widgets._wlayout_dynsim.setOpen(True)
# ----------------------------
# ----------------------------
self._spdsfile = "statepropagatorModel.hdf5"
def _setSPModelFile(x):
self._spdsfile = x
self._wsp_dsfile = kw.StringInput(
router,
"Filename",
"The model filename",
lambda x: _setSPModelFile(x),
rapid_submit=True,
)
self._wsp_dsfile.setValue(self._spdsfile)
self._spdsprefix = "new_"
def _setSPPrefix(x):
self._spdsprefix = x
def _toSPDSCB():
from Karana.Dynamics.SOADyn_types import StatePropagatorDS
ds = StatePropagatorDS.fromStatePropagator(self.item)
ds.toFile(self._spdsfile)
self._wsp_saveds = kw.Button(
router,
text="Save",
on_press=lambda: _toSPDSCB(),
tooltip="Save StatePropagator model to file",
)
self._wsp_layout_model = widgetArray(
router,
label="**Model data**", # self._wsp_md_views,
children=[
self._wsp_saveds,
self._wsp_dsfile,
],
kind="layout", # "inputgroup",
)
self._wsp_layout_ds = widgetArray(
router,
label="StatePropagator model data to file", # self._wst_md_views,
children=[self._wsp_layout_model],
alignment="column",
alignItems="left",
kind="accordion", # "inputgroup",
)
self.wroot.addChild(self._wsp_layout_ds)
self._wsp_layout_ds.setTooltip("Save state propagator model to file.")
self.wroot.addChild(self._wsp_layout_model)
self._wsp_layout_model.setTooltip("Save state propagator model to file.")
def getSummary(self) -> str:
"""Get a markdown summary of the current item."""
result = super().getSummary()
st = self.item
sp = self.item
st = sp.getSubTree()
result += f"subtree='{st.name()}', algorthimically enabled={st.getVars().enabled_algorithmically()}</br>"
result += f"num states={sp.nstates()}, time={km.ktimeToSeconds(sp.getTime())}</br>"
return result
def setup(self, item: kd.StatePropagator, item_context: kw.Json, /):
super().setup(item, item_context)
"""
curr_type = item.getIntegrator().getIntegratorType()
# Setting item early in case setting the below widget states
# triggers a callback relying on self.item to be current.
self.item = item
self._wmaxstep_slider.setValue(
km.ktimeToSeconds(self.item.getMaxStepSize()),
trigger_own_callback=True,
)
if curr_type in self.integ_types:
self._winteg.setIndex(self.integ_types.index(curr_type))
else:
print(f"Warning: unsupported dropdown integrator type {curr_type}")
"""
self._dynsim_widgets.setup(kc.CppWeakRef(item.getSubTree()), kc.CppWeakRef(item))
@register
class IntegratorCard(AbstractCard[ki.Integrator]):
"""Card to display info about any Integrator-derived object."""
@property
def label(self) -> str:
return "Integrator"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
self._wtitle = kw.Markdown(router, text="")
self.wroot.addChild(self._wtitle)
def setup(self, item: ki.Integrator, item_context: kw.Json, /):
super().setup(item, item_context)
def isCompatible(self, item: Any) -> bool:
"""Check whether the Card knows how to display an item."""
# Check if parent compatible and if not another type that is already managed
return super().isCompatible(item) and not isinstance(item, ki.CVodeIntegrator)
@register
class CVodeIntegratorCard(AbstractCard[ki.CVodeIntegrator]):
"""Card to display info about any CVodeIntegrator-derived object."""
@property
def label(self) -> str:
return "CVodeIntegrator"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# Set up sliders for tolerances, etc
slider_opts = kw.SliderOptions()
slider_opts.min = -8
slider_opts.max = -2
slider_opts.log_scale = True
self._atol_slider = kw.Slider(
router, "Atol", lambda new_val: self.item.setAtol(new_val), slider_opts
)
self._rtol_slider = kw.Slider(
router, "Rtol", lambda new_val: self.item.setRtol(new_val), slider_opts
)
slider_opts.min = 1
slider_opts.max = 10
slider_opts.log_scale = False
self._nits_slider = kw.Slider(
router,
"Max NL Iters",
lambda new_val: self.item.setMaxNLIters(int(new_val)),
slider_opts,
)
slider_opts.min = 0
self._alen_slider = kw.Slider(
router,
"Anderson Length",
lambda new_val: self.item.setAndersonLength(int(new_val)),
slider_opts,
)
slider_opts.min = 0
slider_opts.max = 1
slider_opts.step = 0.01
self._adamp_slider = kw.Slider(
router,
"Anderson Damping",
lambda new_val: self.item.setAndersonDamping(new_val),
slider_opts,
)
sliders = [
self._atol_slider,
self._rtol_slider,
self._nits_slider,
self._alen_slider,
self._adamp_slider,
]
for slider in sliders:
slider.setSizeClass(kw.SizeClass.MEDIUM)
self.wroot.addChild(slider)
def setup(self, item: ki.CVodeIntegrator, item_context: kw.Json, /):
super().setup(item, item_context)
opts = cast(ki.CVodeIntegratorOptions, item.getOptions())
self._atol_slider.setValue(opts.atol)
self._rtol_slider.setValue(opts.rtol)
self._nits_slider.setValue(opts.max_nl_iters)
self._alen_slider.setValue(opts.anderson_length)
self._adamp_slider.setValue(opts.anderson_damping)
@register
class BaseKModelCard(AbstractCard[kmdl.BaseKModel]):
"""Card to display info about any BaseKModel-derived object."""
@property
def label(self) -> str:
return "BaseKModel"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# ------------------------
def _toggleDebug(mdl, cstate):
# mdl.debug_model = not mdl.debug_model
mdl.debug_model = cstate
# Update verbosity to be at least DEBUG if it is not already
if mdl.debug_model and kc.MsgLogger.getVerbosity("stdout") > kc.LogLevel.DEBUG:
kc.MsgLogger.changeVerbosity("stdout", kc.LogLevel.DEBUG)
self._wdebug = kw.Toggle(
router,
text="Debug mode",
on_toggle=lambda cstate: _toggleDebug(self.item, cstate),
render_as_button=True,
tooltip="Enable/disable debug mode for the model to display model execution related information to the terminal",
)
# self._wmd_debug = kw.Markdown(router, text="**Debug**", in_line=True)
self._wlayout_debug = widgetArray(
router,
label="Enable/disable debug mode for the model",
children=[self._wdebug],
kind="accordion", # "inputgroup",
)
self._wlayout_debug.setOpen(True)
self._wlayout_debug.setTooltip(
"Enable/disable debug mode for the model to bring out detailed execution information for the model"
)
self.wroot.addChild(self._wlayout_debug)
# ------------------------
def _toggleRegistration(cstate):
mdl = self.item
# reg = mdl.model_manager.getRegisteredModel(mdl.name())
if not cstate:
mdl.model_manager.unregisterModel(mdl)
else:
mdl.model_manager.registerModel(mdl)
self._wregister = kw.Toggle(
router,
text="Enable/diable",
on_toggle=lambda cstate: _toggleRegistration(cstate),
tooltip="Enable/disalbe the model",
)
# Setup widget topology
# self._wmd_active = kw.Markdown(router, text="**Activate**", in_line=True)
self._wlayout_active = widgetArray(
router,
label="Enable/disable the model ",
children=[self._wregister],
kind="accordion", # "inputgroup",
)
self._wlayout_active.setTooltip(
"Enable/disable the model by registering/unregistering it from the state propagator"
)
self.wroot.addChild(self._wlayout_active)
# ------------------------
self._wselect_body = kw.Button(
router,
text="Body",
on_press=lambda: _selectObject(
self.item.multibodyObjs().physical_bodies[0], self.context.selection
),
)
self._wselect_subhinge = kw.Button(
router,
text="Subhinge body",
on_press=lambda: _selectObject(
cast(
kd.PhysicalHinge, self.item.multibodyObjs().physical_subhinges[0].parentHinge()
)
.pnode()
.parentBody(),
self.context.selection,
),
)
self._wselect_node = kw.Button(
router,
text="Node",
on_press=lambda: _selectObject(
self.item.multibodyObjs().nodes[0], self.context.selection
),
)
self._wselect_subtree = kw.Button(
router,
text="Subtree",
on_press=lambda: _selectObject(
self.item.multibodyObjs().subtrees[0], self.context.selection
),
)
# self._wmd_select = kw.Markdown(router, text="**Select**", in_line=True)
self._wlayout_select = widgetArray(
router,
label="Model's multibody objects",
children=[
self._wselect_body,
self._wselect_node,
self._wselect_subtree,
self._wselect_subhinge,
],
kind="accordion",
alignItems="left",
)
self._wlayout_select.setTooltip("Go to the multibody object in use by the model")
self.wroot.addChild(self._wlayout_select)
def setup(self, item: kmdl.BaseKModel, item_context: kw.Json, /):
super().setup(item, item_context)
mbobjs = item.multibodyObjs()
self._wselect_body.setEnabled(len(mbobjs.physical_bodies) > 0)
self._wselect_node.setEnabled(len(mbobjs.nodes) > 0)
self._wselect_subtree.setEnabled(len(mbobjs.subtrees) > 0)
self._wselect_subhinge.setEnabled(len(mbobjs.physical_subhinges) > 0)
is_enabled = item.model_manager.getRegisteredModel(item.name()) is not None
self._wregister.setValue(is_enabled)
self._wdebug.setValue(item.debug_model)
"""
# highlight bodies and nodes invoved with the model
bodies = set()
mbobj = item.multibodyObjs()
bodies.update(mbobj.physical_bodies)
bodies.update([x.parentBody() for x in mbobj.nodes])
for st in mbobj.subtrees:
bodies.update(st.sortedPhysicalBodiesList())
for sh in mbobj.physical_subhinges:
obd = sh.parentHinge().onode().parentBody()
pbd = sh.parentHinge().pnode().parentBody()
bodies.update([obd, pbd])
# hightlight the subtree bodies in the multibody visjs graph
_highlightBodies(
bodies=list(bodies),
secondary_bodies=[],
tertiary_bodies=[],
toggle_over_set=False,
gui_context=self.context,
)
"""
def getSummary(self) -> str:
item = self.item
result = super().getSummary()
result += f"""
period: {km.ktimeToSeconds(item.getPeriod())}s
"""
return result
@register
class PenaltyContactCard(AbstractCard[kmdl.PenaltyContact]):
"""Card to display info about any PIDModel-derived object."""
@property
def label(self) -> str:
return "PenaltyContact"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# --------------------------------
# set up cfv var for future use and make a button
self._cfv: vizutils.ContactForceVisualizer | None = None
def toggleForceViz(active: bool):
# Only register if we've already set up
if self._cfv is None:
return
self.item.setCacheContacts(active)
# Let the cfv register itself with scene
if active:
self._cfv.registerCallback()
# Force redraw with new arrows
self.context.scene.update()
else:
self._cfv.unregisterCallback()
self._wcfv_toggle = kw.Toggle(
router,
"Visualize Contact Forces",
toggleForceViz,
tooltip="Visualize forces resulting from contact",
render_as_button=True,
)
# Default to off
self._wcfv_toggle.setValue(False)
slider_opts = kw.SliderOptions()
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Factor to scale up/down contact forces"
def scaleContact(new_scale):
# Only register if we've already set up
if self._cfv is None:
return
self._cfv.setScale(new_scale)
self._wcfv_scale = kw.Slider(
router,
"Viz Force Scale",
scaleContact,
slider_opts,
)
# Default to unit scale
self._wcfv_scale.setValue(0.05)
self._wlayout_viz = widgetArray(
router,
label="Visualize the model's forces",
children=[self._wcfv_toggle, self._wcfv_scale],
kind="accordion", # "inputgroup",
)
self._wlayout_viz.setOpen(True)
self._wlayout_viz.setTooltip(
"Enable/disable the visualization of the contact force vectors computed by the model"
)
self.wroot.addChild(self._wlayout_viz)
# -------------------------------------------
# Hunt Crossley: kp, kc, n, mu, linear_region_tol
# Hunt Crossley: kp, kc, n, mu, linear_region_tol, dmax, e,
# slider for kp
slider_opts = kw.SliderOptions()
slider_opts.min = 1
slider_opts.max = 6
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Stiffness parameter"
def setkp(val):
cast(kcoll.HuntCrossleyContactForce, self.item.getContactForceModel()).params.kp = val
self._wkp_slider = kw.Slider(
router,
text="kp",
on_change=lambda kp: setkp(kp),
opts=slider_opts,
)
# -------------------------------------------
# slider for kc
slider_opts.tooltip = "Damping parameter"
def setkc(val):
cast(kcoll.HuntCrossleyContactForce, self.item.getContactForceModel()).params.kc = val
self._wkc_slider = kw.Slider(
router,
text="kc",
on_change=lambda kc: setkc(kc),
opts=slider_opts,
)
# -------------------------------------------
# slider for n
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = False
slider_opts.tooltip = "Normal penetration exponent"
def setn(val):
cast(kcoll.HuntCrossleyContactForce, self.item.getContactForceModel()).params.n = val
self._wn_slider = kw.Slider(
router,
text="n",
on_change=lambda n: setn(n),
opts=slider_opts,
)
# -------------------------------------------
# slider for mu
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = False
slider_opts.tooltip = "Coefficient of Friction"
def setmu(val):
cast(kcoll.HuntCrossleyContactForce, self.item.getContactForceModel()).params.mu = val
self._wmu_slider = kw.Slider(
router,
text="mu",
on_change=lambda mu: setmu(mu),
opts=slider_opts,
)
# -------------------------------------------
# slider for linear_region_tol
slider_opts = kw.SliderOptions()
slider_opts.min = -2
slider_opts.max = 2
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Width of linear interpolation band for friction forces"
def setLinRegTol(val):
cast(
kcoll.HuntCrossleyContactForce, self.item.getContactForceModel()
).params.linear_region_tol = val
self._wlinear_region_tol_slider = kw.Slider(
router,
text="linear_region_tol",
on_change=lambda linear_region_tol: setLinRegTol(linear_region_tol),
opts=slider_opts,
)
# -------------------------------------------
# slider for dmax
slider_opts.min = -2
slider_opts.max = 2
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Maximum penetration depth before damping plateaus"
def setdmax(val):
self.item.getContactForceModel().params.dmax = val # pyright: ignore
self._wdmax_slider = kw.Slider(
router,
text="dmax",
on_change=lambda dmax: setdmax(dmax),
opts=slider_opts,
)
# -------------------------------------------
# slider for e
slider_opts = kw.SliderOptions()
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = False
slider_opts.tooltip = "Damping exponent"
def sete(val):
self.item.getContactForceModel().params.e = val # pyright: ignore
self._we_slider = kw.Slider(
router,
text="e",
on_change=lambda e: sete(e),
opts=slider_opts,
)
self._wlayout_info = widgetArray(
router,
label="Contact force parameters",
children=[
self._wkp_slider,
self._wkc_slider,
self._wn_slider,
self._wmu_slider,
self._wlinear_region_tol_slider,
self._wdmax_slider,
self._we_slider,
],
kind="accordion", # "inputgroup",
alignment="column",
alignItems="left",
)
self._wlayout_info.setTooltip("Set the contact force model's parameters")
self.wroot.addChild(self._wlayout_info)
"""
self.wroot.addChild(self._wkp_slider)
self.wroot.addChild(self._wkc_slider)
self.wroot.addChild(self._wn_slider)
self.wroot.addChild(self._wmu_slider)
self.wroot.addChild(self._wlinear_region_tol_slider)
self.wroot.addChild(self._wdmax_slider)
self.wroot.addChild(self._we_slider)
"""
def setup(self, item: kmdl.PenaltyContact, item_context: kw.Json, /):
super().setup(item, item_context)
# Lazy-setup of cfv
if self._cfv is None:
# Default to off, so no registration yet
self._cfv = vizutils.ContactForceVisualizer(
"cfv", self.context.multibody.virtualRoot(), self.context.scene, item
)
self._cfv.setRadius(0.05)
frcmdl = item.getContactForceModel()
from Karana.Collision import HuntCrossleyContactForce, DampedContactForce
possible_params = ["kp", "kc", "n", "mu", "linear_region_tol", "dmax", "e"]
params_damped = possible_params
params_hc = ["kp", "kc", "n", "mu", "linear_region_tol"]
params_dict = {p: False for p in possible_params}
# Set-specific values
if isinstance(frcmdl, DampedContactForce):
# Set initial values
self._we_slider.setValue(frcmdl.params.e)
self._wdmax_slider.setValue(frcmdl.params.dmax)
self._wkp_slider.setValue(frcmdl.params.kp)
self._wkc_slider.setValue(frcmdl.params.kc)
self._wn_slider.setValue(frcmdl.params.n)
self._wmu_slider.setValue(frcmdl.params.mu)
self._wlinear_region_tol_slider.setValue(frcmdl.params.linear_region_tol)
# Mark active params
for k in params_damped:
params_dict[k] = True
if isinstance(frcmdl, HuntCrossleyContactForce):
# TODO for @kelly can we avoid code duplication from above
# Set initial values
self._wkp_slider.setValue(frcmdl.params.kp)
self._wkc_slider.setValue(frcmdl.params.kc)
self._wn_slider.setValue(frcmdl.params.n)
self._wmu_slider.setValue(frcmdl.params.mu)
self._wlinear_region_tol_slider.setValue(frcmdl.params.linear_region_tol)
# Mark active params
for k in params_hc:
params_dict[k] = True
self._wkp_slider.setVisible(params_dict["kp"])
self._wkc_slider.setVisible(params_dict["kc"])
self._wn_slider.setVisible(params_dict["n"])
self._wmu_slider.setVisible(params_dict["mu"])
self._wlinear_region_tol_slider.setVisible(params_dict["linear_region_tol"])
self._wdmax_slider.setVisible(params_dict["dmax"])
self._we_slider.setVisible(params_dict["e"])
@register
class SpringDamperCard(AbstractCard[kmdl.SpringDamper]):
"""Card to display info about any PIDModel-derived object."""
@property
def label(self) -> str:
return "SpringDamper"
def __init__(self, context: GuiContext):
super().__init__(context)
# Create widgets
router = context.router
# Hunt Crossley: kp, kc, n, mu, linear_region_tol
# Hunt Crossley: kp, kc, n, mu, linear_region_tol, dmax, e,
# -------------------------------------------
# slider for kp
slider_opts = kw.SliderOptions()
slider_opts.min = 1
slider_opts.max = 6
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Stiffness parameter"
def setk(val):
self.item.params.k = val
self._wk_slider = kw.Slider(
router,
text="k",
on_change=lambda k: setk(k),
opts=slider_opts,
)
# -------------------------------------------
# slider for kc
slider_opts.tooltip = "Damping parameter"
def setd(val):
self.item.params.d = val
self._wd_slider = kw.Slider(
router,
text="d",
on_change=lambda d: setd(d),
opts=slider_opts,
)
# set up nd1v var for future use and make a button
self._nd1v: vizutils.ScaledVectorVisualizer | None = None
self._nd2v: vizutils.ScaledVectorVisualizer | None = None
def toggleForceViz(active: bool):
# Only register if we've already set up
if self._nd1v is None or self._nd2v is None:
return
# Let the nd1v register itself with scene
if active:
self._nd1v.registerCallback()
self._nd2v.registerCallback()
# Force redraw with new arrows
self.context.scene.update()
else:
self._nd1v.unregisterCallback()
self._nd2v.unregisterCallback()
self._wndv_toggle = kw.Toggle(
router,
"Visualize Forces",
toggleForceViz,
tooltip="Visualize the spring damper forces at the nodes",
render_as_button=True,
)
# Default to off
self._wndv_toggle.setValue(False)
slider_opts = kw.SliderOptions()
slider_opts.min = -3
slider_opts.max = 3
slider_opts.step = 0.01
slider_opts.log_scale = True
slider_opts.tooltip = "Factor to scale up/down forces"
def scaleForces(new_scale):
# Only register if we've already set up
if self._nd1v is None or self._nd2v is None:
return
self._nd1v.setScale(new_scale)
self._nd2v.setScale(new_scale)
self._wndv_scale = kw.Slider(
router,
"Viz Force Scale",
scaleForces,
slider_opts,
)
# Default to unit scale
self._wndv_scale.setValue(0.05)
self.wroot.addChild(self._wk_slider)
self.wroot.addChild(self._wd_slider)
self.wroot.addChild(self._wndv_toggle)
self.wroot.addChild(self._wndv_scale)
def setup(self, item: kmdl.SpringDamper, item_context: kw.Json, /):
super().setup(item, item_context)
# Lazy-setup of nd1v
if self._nd1v is None:
mbobjs = item.multibodyObjs()
# Default to off, so no registration yet
self._nd1v = vizutils.visualizeNodeForce(
mbobjs.nodes[0],
self.context.scene,
)
self._nd2v = vizutils.visualizeNodeForce(
mbobjs.nodes[1],
self.context.scene,
)
# @register
# class PIDModelCard(AbstractCard[kmdl.PID]):
# """Card to display info about any PIDModel-derived object."""
# @property
# def label(self) -> str:
# return "PIDModel"
# def __init__(self, context: GuiContext):
# super().__init__(context)
# # Create widgets
# router = context.router
# self._wshow_body = kw.Button(
# router,
# text="Show body",
# on_press=lambda: _highlightBodies(
# bodies=[
# self.item.multibodyObjs()
# .physical_subhinges[0]
# .parentHinge()
# .pnode()
# .parentBody()
# ],
# secondary_bodies=[],
# tertiary_bodies=[],
# gui_context=self.context,
# ),
# )
# """
# self._wselect_body = kw.Button(
# router,
# text="Select body",
# on_press=lambda: _selectObject(
# self.item.multibodyObjs().physical_subhinges[0].parentHinge().pnode().parentBody(),
# self.context.selection,
# ),
# )
# """
# # Setup widget topology
# # self._wmd_gravity = kw.Markdown(router, text="**Gravity**")
# # self.wroot.addChild(self._wmd_gravity)
# self.wroot.addChild(self._wshow_body)
# #self.wroot.addChild(self._wselect_body)
# def updateFor(self, item: kf.Frame, item_context: kw.Json):
# pass
class InfoPanel:
@property
def context(self) -> GuiContext:
return self._context
def _stepActiveCard(self, incr: int):
"""Switch to the card incr steps forward from the active card."""
compatible_cards = [card for card in self._cards if card.isCompatible(self._item)]
# We need a nonempty list for all of this to work, so catch it
# early in case this method somehow gets called in this state.
if not compatible_cards:
kc.warn("There are no cards compatible with the current item")
return
try:
active_card_index = compatible_cards.index(self._active_card)
# Compute next card index, wrapping around at the end
new_card_index = (active_card_index + incr) % len(compatible_cards)
except ValueError:
# Happens if the active card somehow isn't compatible with
# the current item.
kc.warn("Cannot determine index of the active card")
# Just reset back to the first card to get back into a valid
# state
new_card_index = 0
# Treat this card switch as a user request so that the choice
# is sticky
self._requestActivateCard(compatible_cards[new_card_index])
def __init__(
self,
context: GuiContext,
cards: Iterable[AbstractCard] | None = None,
):
"""Create a new InfoPanel."""
self._context = context
router = context.router
self._wroot = kw.Layout(router, style={"height": "100%"})
# Container for content when nothing is selected
self._wdefault = kw.Layout(router, style={"padding": "0.5em"})
self._wdefault_text = kw.Markdown(router, text="*No items selected*")
# Container for content when something is selected
self._wselection = kw.Layout(router)
self._wselection.addDomClass("selection-panel")
self._wselection.setVisible(False)
# Button to refresh the active card (eg: to update numerics)
self._wrefresh = kw.Button(
router,
text="Refresh",
on_press=lambda: self.updateFor(self.getItem(), self._item_context),
tooltip=f"Refresh the contents of this card",
)
# Container for the button widgets to select the active card
self._wbuttons = kw.Layout(router)
self._wbuttons.addDomClass("selection-buttons")
# Container for the card widgets
self._wcards_area = kw.Layout(router)
self._wcards_area.addDomClass("selection-cards")
self._wcards = kw.Layout(router)
self._button_list = []
"""
self._wprev_card = kw.Button(
router, text="Card Up ↑", on_press=lambda: self._stepActiveCard(-1)
)
self._wnext_card = kw.Button(
router, text="Card Down ↓", on_press=lambda: self._stepActiveCard(1)
)
"""
if cards is None:
# By default use all card types that were registered,
# sorting based on class hierarchy of their wrapped types,
# with less derived coming first.
self._cards = [cls(context) for cls in self._hierarchySort(known_card_types)]
else:
self._cards = list(cards)
# Context for the currently selected item
self._item_context = None
# This is the last card the user requested by clicking a button.
# If this card is valid for the current item then it should be
# prioritized. This makes the user card selection 'sticky', so
# that if the user selects a card, selects an incompatible
# item, then goes back to compatible items, the user-selected
# card isn't forgotten.
self._user_card = None
# This is the currently displayed card. It may differ from the
# user card if the user card isn't valid for the current
# selection.
self._active_card = None
# Creates a no-argument callable with a card bound to its
# closure. This will be used to generate callbacks for buttons,
# each using a distinct card.
def activateCardClosure(card: AbstractCard):
def activateCardClosureInner():
self._requestActivateCard(card)
return activateCardClosureInner
for card in self._cards:
# Hide all cards initially
card.wroot.setVisible(False)
# Create the buttons to activate each card
on_press = activateCardClosure(card)
wbutton = kw.Button(
router,
text=card.label,
on_press=on_press,
tooltip=f"Switch to the '{card.label}' base class card",
)
wbutton.addDomClass("karana-card-choice")
self._button_list.append(wbutton)
# Setup widget topology
self._wdefault.addChild(self._wdefault_text)
self._wbuttons.addChild(self._wrefresh)
for wbutton in self._button_list:
self._wbuttons.addChild(wbutton)
for card in self._cards:
self._wcards.addChild(card.wroot)
# self._wcards_area.addChild(self._wprev_card)
self._wcards_area.addChild(self._wcards)
# self._wcards_area.addChild(self._wnext_card)
self._wselection.addChild(self._wbuttons)
self._wselection.addChild(self._wcards_area)
self._wroot.addChild(self._wdefault)
self._wroot.addChild(self._wselection)
def getItem(self) -> Any | None:
return getattr(self, "_item", None)
def _requestActivateCard(self, card: AbstractCard):
"""Request from user that a card should be activated.
This will store the requested card so that it will be active
whenever it is compatible with the currently selected item.
"""
self._user_card = card
# Only allow a card if it's compatible with the current item,
# typically meaning it's for the current item's type or a
# subclass.
if not card.isCompatible(self.getItem()):
return
self._activateCard(card)
def _activateCard(self, card: AbstractCard | None):
"""Display the given card.
If card is None, hide all cards. This can happen in the edge
case where the selected item has no compatible cards. Note
that this function assumes the card is compatible with the
current item, so this must be checked before calling.
"""
if self._active_card == card:
return
old_card = self._active_card
self._active_card = card
# Update card selection button styling so the one for the
# active card is visually distinct
for wbutton, candidate_card in zip(self._button_list, self._cards, strict=True):
if candidate_card == old_card:
wbutton.removeDomClass("karana-card-choice-selected")
if candidate_card == self._active_card:
wbutton.addDomClass("karana-card-choice-selected")
if card is None:
# Only doing this now since we are about to return.
# Otherwise we should wait until the new card is ready to
# display to minimize the amount of time that no card is
# visible.
# We don't need to check old_card is not None, because if it was None,
# then old_card == card and we would have returned above
cast(AbstractCard, old_card).wroot.setVisible(False)
return
# Update the new card and switch to it
try:
card.updateFor(self.getItem(), self._item_context)
except Exception:
cls_name = type(card).__name__
msg = f"Error updating {cls_name}:\n{traceback.format_exc()}"
kc.error(msg)
self.context.signal_error()
finally:
card.item = self.getItem()
if old_card is not None:
# Teardown the item on the old card to clean up any side-effects
if old_item := old_card.getItem():
old_card.teardown(old_item)
old_card.wroot.setVisible(False)
card.wroot.setVisible(True)
@property
def wroot(self) -> kw.Widget:
"""Get the root widget."""
return self._wroot
def updateFor(self, item: Any | None, item_context: kw.Json):
"""Update the active card for the given item."""
old_item = self.getItem()
self._item = item
self._item_context = item_context
if item is None:
if old_item is None:
return
# We are deselecting, so switch to the default widget and
# deactivate any active card
self._wselection.setVisible(False)
self._wdefault.setVisible(True)
self._activateCard(None)
return
# Might need to switch cards based on compatibility. First check
# if the user's preferred card is compatible with the new item.
new_active_card = None
if self._user_card and self._user_card.isCompatible(item):
new_active_card = self._user_card
if new_active_card is None:
# Use the most derived card that is compatible
for card in reversed(self._cards):
if card.isCompatible(item):
new_active_card = card
break
# At this point, new_active_card could still be None if
# no card is compatible. This is supported by
# _activateCard and should hide all cards.
if new_active_card is not None and new_active_card == self._active_card:
try:
cast(AbstractCard, self._active_card).updateFor(item, item_context)
except Exception:
cls_name = type(self).__name__
msg = f"Error updating {cls_name}:\n{traceback.format_exc()}"
kc.error(msg)
self.context.signal_error()
finally:
cast(AbstractCard, self._active_card).item = item
else:
self._activateCard(new_active_card)
# Only show activation buttons for compatible cards
for wbutton, card in zip(self._button_list, self._cards, strict=True):
wbutton.setVisible(card.isCompatible(item))
# If there wasn't a selection prior we need to switch to
# the selection widget
if old_item is None:
self._wdefault.setVisible(False)
self._wselection.setVisible(True)
@staticmethod
def _hierarchySort(types: Iterable[type]) -> Sequence[type]:
"""Sort the given types so that less derived ones come first.
We sort first by the length of each type's mro (method
resolution order) in ascending order, meaning that less
derived types will come earlier. We use the item's position
in the original iterator as a tie-breaker to preserve order
where possible.
"""
# Build a list of (index, type) tuples
items = list(enumerate(types))
def keyFunc(item):
index, type_ = item
# This sorts first by ascending mro length, then by index
return (len(type_.__mro__), index)
# Sort the list of (index, type) tuples
items.sort(key=keyFunc)
# Extract the types in order from the sorted item tuples
return [item[1] for item in items]
def addCard(self, card: AbstractCard):
"""Add an AbstractCard to be shown for compatible items."""
# Add the card to our bookkept list of cards
self._cards.append(card)
card.wroot.setVisible(False)
# Create the buttons to activate the card
wbutton = kw.Button(
self.context.router,
text=card.label,
on_press=lambda: self._requestActivateCard(card),
tooltip=f"Switch to the '{card.label}' base class card",
)
wbutton.addDomClass("karana-card-choice")
# Only show the button right away if its card is compatible with the current item
wbutton.setVisible(card.isCompatible(self.getItem()))
# Add the button to our bookkept list of card buttons
self._button_list.append(wbutton)
# Add the button to the layout widget so it can be seen on the frontend
self._wbuttons.addChild(wbutton)
# Add the card to the layout widget so it can be seen on the frontend
self._wcards.addChild(card.wroot)
def close(self):
"""Do any necessary cleanup."""
self.updateFor(None, None)
for card in self._cards:
card.close()
self._cards = []
