# Copyright (c) 2024-2025 Karana Dynamics Pty Ltd. All rights reserved.
#
# NOTICE TO USER:
#
# This source code and/or documentation (the "Licensed Materials") is
# the confidential and proprietary information of Karana Dynamics Inc.
# Use of these Licensed Materials is governed by the terms and conditions
# of a separate software license agreement between Karana Dynamics and the
# Licensee ("License Agreement"). Unless expressly permitted under that
# agreement, any reproduction, modification, distribution, or disclosure
# of the Licensed Materials, in whole or in part, to any third party
# without the prior written consent of Karana Dynamics is strictly prohibited.
#
# THE LICENSED MATERIALS ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND.
# KARANA DYNAMICS DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING
# BUT NOT LIMITED TO WARRANTIES OF MERCHANTABILITY, NON-INFRINGEMENT, AND
# FITNESS FOR A PARTICULAR PURPOSE.
#
# IN NO EVENT SHALL KARANA DYNAMICS BE LIABLE FOR ANY DAMAGES WHATSOEVER,
# INCLUDING BUT NOT LIMITED TO LOSS OF PROFITS, DATA, OR USE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGES, WHETHER IN CONTRACT, TORT,
# OR OTHERWISE ARISING OUT OF OR IN CONNECTION WITH THE LICENSED MATERIALS.
#
# U.S. Government End Users: The Licensed Materials are a "commercial item"
# as defined at 48 C.F.R. 2.101, and are provided to the U.S. Government
# only as a commercial end item under the terms of this license.
#
# Any use of the Licensed Materials in individual or commercial software must
# include, in the user documentation and internal source code comments,
# this Notice, Disclaimer, and U.S. Government Use Provision.
"""Convert DARTS model files to MultibodyDS.
This module contains tools to convert a DARTS model file to a MultibodyDS. Use the
convertDARTSModelFile to do so.
"""
import ast
import numpy as np
from collections import defaultdict
from pathlib import Path
from Karana.Dynamics.SOADyn_types import (
MultibodyDS,
BodyWithContextDS,
BodyDS,
LoopConstraintHingeDS,
NodeDS,
HingeDS,
HINGE_TYPE,
PinSubhingeDS,
Linear3SubhingeDS,
SphericalSubhingeDS,
LinearSubhingeDS,
LockedSubhingeDS,
)
from Karana.Core import warn
from Karana.Math import UnitQuaternion, SpatialInertia, HomTran
from typing import Any
class _CreateMbodiesVisitor(ast.NodeVisitor):
"""Visit nodes of an AST and extract the value associatd with the "darts" key of the last createMbodies call."""
def __init__(self):
"""Create _CreateMbodiesVisitor class.."""
self.darts_value = {}
def visit_Call(self, node):
"""Visit call that retreives the darts value from createMbodies."""
# Check if this is a call to a method named 'createMbodies'
if isinstance(node.func, ast.Attribute) and node.func.attr == "createMbodies":
# Check if first argument is a dict
if node.args and isinstance(node.args[0], ast.Dict):
dict_node = node.args[0]
for key_node, value_node in zip(dict_node.keys, dict_node.values):
if isinstance(key_node, ast.Constant) and key_node.value == "darts":
self.darts_value = ast.literal_eval(value_node)
self.generic_visit(node)
class _TreeBuilder:
"""Create tree of bodies from DARTS body parameter dictionaries.
This class builds up a tree over time (via calls to the addBody method). Once
all bodies have been added, the getBodiesWithContext is used to recurse the
structure and create the BodyWithContextDS for all the base bodies.
"""
def __init__(self):
"""Create _TreeBuilder class."""
# This holds the bodies that have been created so far
self.bodies: dict[str, BodyDS] = {}
self.loop_constraints: list[LoopConstraintDS] = []
# This keeps track of the parents for each body
self.body_parents: dict[str, str] = {}
# This holds the names of bodies in self.bodies that have a parent defined,
# but the parent has not been created yet. The key is the body name and
# the value is the parent_name
self.awaiting_parent: dict[str, str] = {}
def addBody(self, body_name: str, body_dict: dict[str, Any]) -> None:
"""
Add a body _TreeBuilder's list of bodies.
Parameters
----------
body_name : str
The name of the body to add to _TreeBuilder.
body_dict : dict[str, Any]
The DARTS body parameters for the body.
"""
body = self._createBody(body_name, body_dict)
parent_name = body_dict.get("inbBody", None)
if body.name in self.bodies:
raise ValueError("Cannot handle multiple bodies with the same name.")
# Add this body to the list of bodies
self.bodies[body.name] = body
if parent_name is not None:
self.body_parents[body.name] = parent_name
if parent_name is not None and parent_name not in self.bodies:
self.awaiting_parent[body.name] = parent_name
# List of keys to remove from awaiting_parents, since these bodies' parent has been added
# Need to collect these first, as popping invalidates iterators
pop_keys = [k for k, v in self.awaiting_parent.items() if v == body.name]
# Remove any bodies that were awaiting this body as their parent
for k in pop_keys:
self.awaiting_parent.pop(k)
def addLoopConstraint(self, lc_name: str, lc_dict: dict[str, Any]) -> None:
"""
Add a loop constraint.
Parameters
----------
lc_name : str
The name of the loop constraint to add to _TreeBuilder.
lc_dict : dict[str, Any]
The DARTS loop constraint parameters.
"""
lc = self._createLoopConstraint(lc_name, lc_dict)
if lc.name in self.loop_constraints:
raise ValueError("Cannot handle multiple constraints with the same name.")
# Add this body to the list of bodies
self.loop_constraints.append(lc)
def getBodiesWithContext(self) -> list[BodyWithContextDS]:
"""Retrieve BodyWithContextDS for all bodies in _TreeBuilder.
This traverses the tree that has been created from all the addBody calls.
It creates the recursive BodyWithContextDS structures.
Returns
-------
list[BodyWithContextDS]
A list of all BodyWithContextDS for the base bodies
"""
# Invert the parent_map
children_map: dict[str, list[str]] = defaultdict(list)
for child, parent in self.body_parents.items():
children_map[parent].append(child)
# Identify root nodes
all_keys = set(self.bodies.keys())
children_keys = set(self.body_parents.keys())
root_keys = all_keys - children_keys
# Sort the root_keys so their order matches their original order
body_keys = [k for k in self.bodies.keys()]
root_keys = sorted(root_keys, key=lambda x: body_keys.index(x))
# Recursive construction of BodyWithContextDS
def buildContext(key: str) -> BodyWithContextDS:
children_bodies = [buildContext(child_key) for child_key in children_map.get(key, [])]
return BodyWithContextDS(body=self.bodies[key], children=children_bodies)
# Build the full structure from the roots
return [buildContext(root_key) for root_key in root_keys]
@staticmethod
def _createLoopConstraint(lc_name: str, lc_dict: dict[str, Any]) -> LoopConstraintHingeDS:
"""Create a LoopConstraintHingeDS from the provided inputs.
Parameters
----------
lc_name : str
Name of the loop constraint.
lc_dict : dict[str,Any]
DARTS constraint parameter dictionary.
Returns
-------
LoopConstraintHingeDS
A LoopConstraintHingeDS that contains a DataStruct for the loop constraint.
"""
prescribed = lc_dict.get("prescribed", False)
joint_type = lc_dict.get("jointType", None)
joint_axes = [np.array(x) for x in lc_dict.get("jointAxes", [])]
subhinge_types = [] # need to populate for custom hinges
hinge_ds = _toHingeDs(joint_type, joint_axes, prescribed, subhinge_types)
return LoopConstraintHingeDS(
name=lc_name,
hinge=hinge_ds,
get_oframe={"frame": lc_dict["sourceNode"]},
get_pframe={"frame": lc_dict["targetNode"]},
)
@staticmethod
def _createBody(body_name: str, body_dict: dict[str, Any]) -> BodyDS:
"""Create a BodyDS from the provided inputs.
Parameters
----------
body_name : str
Name of the body.
body_dict : dict[str,Any]
DARTS body parameter dictionary.
Returns
-------
BodyDS
A BodyDS that contains a DataStruct for the body.
"""
# Determine if this is a flex body. If so, grab flex-body-specific quantities
n_modes = body_dict.get("modes", 0)
if n_modes > 0:
flex_body = True
stiffness_vector = body_dict.get("stiffnessVector", None)
if stiffness_vector is None:
warn(f"Stiffness vector is not set for {body_name}. Setting it to all zeros.")
stiffness_vector = np.zeros(n_modes)
else:
stiffness_vector = np.array(stiffness_vector)
damping_vector = np.array(
body_dict.get(
"dampingVector",
[
0,
]
* n_modes,
)
)
body_nodal_matrix = body_dict.get("bodyNodalMatrix", None)
if body_nodal_matrix is None:
if body_dict.get("jointType", None) not in ["FULL6DOF", "FULL6DOF_INERTIAL"]:
warn(
f"Nodal matrix for {body_name} is not set, and the joint type is not 6 DoF. Setting to all zeros."
)
body_nodal_matrix = np.zeros((6, n_modes))
else:
body_nodal_matrix = np.array(body_nodal_matrix)
else:
flex_body = False
# Get quantities common to all bodies
mass = body_dict.get("mass", 0)
b2cm = np.array(
body_dict.get(
"bodyToCM",
[
0,
]
* 3,
)
)
if "cmInertia" in body_dict:
cmI = body_dict["cmInertia"]
cmspi = SpatialInertia(mass, [0, 0, 0], cmI)
spi = cmspi.parallelAxis(HomTran(b2cm))
inertia = spi.inertia()
else:
inertia = np.array(body_dict["inertia"])
body_to_joint = np.array(
body_dict.get(
"bodyToJoint",
[
0,
]
* 3,
)
)
body_to_joint_quat = UnitQuaternion(*body_dict.get("bodyToJointQuat", [0, 0, 0, 1]))
inb_to_joint = np.array(
body_dict.get(
"inbToJoint",
[
0,
]
* 3,
)
)
inb_to_joint_quat = UnitQuaternion(*body_dict.get("inbToJointQuat", [0, 0, 0, 1]))
inb_nodal_matrix = body_dict.get("inbNodalMatrix", None)
if inb_nodal_matrix is not None:
inb_nodal_matrix = np.array(inb_nodal_matrix)
visuals = []
# Get node information
sensor_nodes: list[NodeDS] = []
force_nodes: list[NodeDS] = []
constraint_nodes: list[NodeDS] = []
if flex_body:
# Flexible nodes
from Karana.Dynamics.SOAFlexDyn_types import ModalNodeDS
for name, sn in body_dict.get("SENSOR_NODES", {}).items():
nodal_matrix = np.array(sn.get("nodalMatrix", None))
if nodal_matrix is None:
warn(f"Nodal matrix for sensor node {name} of body {body_name} is not set.")
nodal_matrix = np.zeros((6, n_modes))
sensor_nodes.append(
ModalNodeDS(
name=name,
force_node=False,
constraint_node=False,
translation=np.array(
sn.get(
"bodyToNode",
[
0,
]
* 3,
)
),
unit_quaternion=UnitQuaternion(*sn.get("bodyToJointQuat", [0, 0, 0, 1])),
nodal_matrix=nodal_matrix,
)
)
for name, an in body_dict.get("ACTUATOR_NODES", {}).items():
nodal_matrix = np.array(an.get("nodalMatrix", None))
if nodal_matrix is None:
warn(f"Nodal matrix for actuator node {name} of body {body_name} is not set.")
nodal_matrix = np.zeros((6, n_modes))
force_nodes.append(
ModalNodeDS(
name=name,
force_node=True,
constraint_node=False,
translation=np.array(
an.get(
"bodyToNode",
[
0,
]
* 3,
)
),
unit_quaternion=UnitQuaternion(*an.get("bodyToJointQuat", [0, 0, 0, 1])),
nodal_matrix=nodal_matrix,
)
)
for name, an in body_dict.get("CONSTRAINT_NODES", {}).items():
nodal_matrix = np.array(an.get("nodalMatrix", None))
if nodal_matrix is None:
warn(f"Nodal matrix for constraint node {name} of body {body_name} is not set.")
nodal_matrix = np.zeros((6, n_modes))
constraint_nodes.append(
ModalNodeDS(
name=name,
force_node=True,
constraint_node=True,
translation=np.array(
an.get(
"bodyToNode",
[
0,
]
* 3,
)
),
unit_quaternion=UnitQuaternion(*an.get("bodyToNodeQuat", [0, 0, 0, 1])),
nodal_matrix=nodal_matrix,
)
)
else:
# Rigid-body nodes
for name, sn in body_dict.get("SENSOR_NODES", {}).items():
sensor_nodes.append(
NodeDS(
name=name,
force_node=False,
constraint_node=False,
translation=np.array(
sn.get(
"bodyToNode",
[
0,
]
* 3,
)
),
unit_quaternion=UnitQuaternion(*sn.get("bodyToJointQuat", [0, 0, 0, 1])),
)
)
for name, an in body_dict.get("ACTUATOR_NODES", {}).items():
force_nodes.append(
NodeDS(
name=name,
force_node=True,
constraint_node=False,
translation=np.array(
an.get(
"bodyToNode",
[
0,
]
* 3,
)
),
unit_quaternion=UnitQuaternion(*an.get("bodyToJointQuat", [0, 0, 0, 1])),
)
)
for name, an in body_dict.get("CONSTRAINT_NODES", {}).items():
constraint_nodes.append(
NodeDS(
name=name,
force_node=True,
constraint_node=True,
translation=np.array(
an.get(
"bodyToNode",
[
0,
]
* 3,
)
),
unit_quaternion=UnitQuaternion(*an.get("bodyToNodeQuat", [0, 0, 0, 1])),
)
)
# Hinge information
prescribed = body_dict.get("prescribed", False)
joint_type = body_dict.get("jointType", None)
joint_axes = [np.array(x) for x in body_dict.get("jointAxes", [])]
subhinge_types = [] # need to populate for custom hinges
hinge = _toHingeDs(joint_type, joint_axes, prescribed, subhinge_types)
if flex_body:
from Karana.Dynamics.SOAFlexDyn_types import ModalBodyDS
return ModalBodyDS(
name=body_name,
mass=mass,
b2cm=b2cm,
inertia=inertia,
hinge=hinge,
body_to_joint=body_to_joint,
body_to_joint_quat=body_to_joint_quat,
inb_to_joint=inb_to_joint,
inb_to_joint_quat=inb_to_joint_quat,
inb_nodal_matrix=inb_nodal_matrix,
sensor_nodes=sensor_nodes,
force_nodes=force_nodes,
constraint_nodes=constraint_nodes,
visuals=visuals,
stiffness_vector=stiffness_vector, # pyright: ignore - Will be defined if flex_body is True
damping_vector=damping_vector, # pyright: ignore - Will be defined if flex_body is True
nodal_matrix=body_nodal_matrix, # pyright: ignore - Will be defined if flex_body is True
)
else:
return BodyDS(
name=body_name,
mass=mass,
b2cm=b2cm,
inertia=inertia,
hinge=hinge,
body_to_joint=body_to_joint,
body_to_joint_quat=body_to_joint_quat,
inb_to_joint=inb_to_joint,
inb_to_joint_quat=inb_to_joint_quat,
inb_nodal_matrix=inb_nodal_matrix,
sensor_nodes=sensor_nodes,
force_nodes=force_nodes,
constraint_nodes=constraint_nodes,
visuals=visuals,
)
def _toHingeDs(
joint_type: str, joint_axes: list, prescribed: bool, subhinge_types: list
) -> HingeDS:
if joint_type in ["FULL6DOF_INERTIAL", "FULL6DOF"]:
hinge = HingeDS(
hinge_type=HINGE_TYPE.FULL6DOF,
subhinges=[
Linear3SubhingeDS(prescribed=prescribed),
SphericalSubhingeDS(prescribed=prescribed),
],
)
if joint_type == "FULL6DOF":
warn(f"Using FULL6DOF hinge - will need to convert velocity values accordingly.")
elif joint_type == "BALL":
hinge = HingeDS(
hinge_type=HINGE_TYPE.BALL, subhinges=[SphericalSubhingeDS(prescribed=prescribed)]
)
elif joint_type == "GIMBAL":
hinge = HingeDS(
hinge_type=HINGE_TYPE.GIMBAL,
subhinges=[
PinSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[0]),
PinSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[1]),
PinSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[2]),
],
)
elif joint_type == "LOCKED":
hinge = HingeDS(
hinge_type=HINGE_TYPE.LOCKED,
subhinges=[LockedSubhingeDS(prescribed=False)],
)
elif joint_type == "PIN":
hinge = HingeDS(
hinge_type=HINGE_TYPE.PIN,
subhinges=[PinSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[0])],
)
elif joint_type == "PLANAR":
hinge = HingeDS(
hinge_type=HINGE_TYPE.PLANAR,
subhinges=[
LinearSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[0]),
LinearSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[1]),
],
)
elif joint_type == "SLIDER":
hinge = HingeDS(
hinge_type=HINGE_TYPE.SLIDER,
subhinges=[LinearSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[0])],
)
elif joint_type == "TRANSLATIONAL":
hinge = HingeDS(
hinge_type=HINGE_TYPE.TRANSLATIONAL,
subhinges=[Linear3SubhingeDS(prescribed=prescribed)],
)
elif joint_type == "UJOINT":
hinge = HingeDS(
hinge_type=HINGE_TYPE.UJOINT,
subhinges=[
PinSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[0]),
PinSubhingeDS(prescribed=prescribed, unit_axis=joint_axes[1]),
],
)
elif joint_type is None:
raise ValueError(f"Cannot handle none joint type.")
else:
raise ValueError(f'Not sure what to do with jointType "{joint_type}".')
return hinge
[docs]
def convertDARTSModelFile(filename: Path | str) -> MultibodyDS:
"""Convert a DARTS model file into a MultibodyDS.
Parameters
----------
filename : Path|str
The DARTS model file to convert.
Returns
-------
MultibodyDS
The MultibodyDS that cooresponds with the provided DARTS model file.
"""
if isinstance(filename, str):
filename = Path(filename)
with open(str(filename), "r") as f:
tree = ast.parse(f.read(), filename=filename)
# Extract the value associated with the "darts" key of the createMbodies call.
visitor = _CreateMbodiesVisitor()
visitor.visit(tree)
bodies = visitor.darts_value.get("BODIES", {})
tb = _TreeBuilder()
for name, body in bodies.items():
tb.addBody(name, body)
loops = visitor.darts_value.get("CLOSURE_CONSTRAINTS", {})
for name, lc in loops.items():
tb.addLoopConstraint(name, lc)
return MultibodyDS(
name="mb",
base_bodies=tb.getBodiesWithContext(),
loop_constraints=tb.loop_constraints,
# constraint_frames=tb.constraint_frames
)
