# 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 a FA model into a TA model.
Consider converting to C++.
"""
__all__ = ["TreeConverter"]
from collections.abc import Mapping, Container
from Karana.Dynamics import (
HINGE_TYPE,
LoopConstraintHinge,
LoopConstraintBase,
PhysicalBody,
)
from Karana.Core import discard
from Karana.Dynamics import Multibody
[docs]
class TreeConverter:
"""Class to do an FA to TA conversion on a Multibody
Whenever a hinge-based loop constraint is between a pair of bodies where at
least one of the bodies is FULL6DOF, the loop constraint can be converted
to a regular hinge. Using this approach, this class can greedily convert
fully or partially FA models to TA models with a minimal number of loop
constraints, reducing the total number of constraints and degrees of
freedom in the system.
"""
def __init__(
self,
parent_prefs: Mapping[int, int] | None = None,
skip_prefs: Mapping[int, int] | None = None,
verbose: bool = False,
):
"""TreeConverter constructor
Parameters
----------
parent_prefs: Mapping[int, int] | None
A-priori preferences for parents. The keys are body ids, and
the values the preferred parent ids.
skip_prefs: Mapping[int, int] | None
The loop constraint conversions to skip. The key/value ids
for the body pair whose loop constraint conversion should be
skipped.
verbose: bool
If True, enable verbose output throughout the conversion
process. Defaults to False.
"""
self.donebds = []
self.skipbds = []
self.donelcs = []
# map a-priori preference for parents. The keys are body ids, and the
# values the preferred parent ids
self.parent_prefs = parent_prefs | {}
# map for the loop constraint conversions to skip. The key/values ids
# for the body pair whose loop constraint conversion should be skipped
self.skip_prefs = skip_prefs | {}
self.todo = []
self.visited = []
self.verbose = verbose
[docs]
def canConvertToHinge(self, lc: LoopConstraintBase, basebds: Container[PhysicalBody]):
"""Whether the loop contraint can be converted to a hinge
It must be hinge-based an have at least one free body
"""
# can only convert hinge-based loop constraints
if not lc.hasHinge():
return False
# skip CUSTOM hinge constraints for now since they likely came from
# JPRIM. We do not want them to be used in the initial topology
# creation. We can always do another pass at the end in case any of
# these are convertable into hinges.
if lc.hinge().hingeType() == HINGE_TYPE.CUSTOM:
# print("SKIPPING IN FIRST PASS", lc.name(), len(second_pass_lcs))
# second_pass_lcs.append(lc)
return False
snd = lc.sourceNode()
tnd = lc.targetNode()
sbd = snd.parentBody() if snd else None
tbd = tnd.parentBody() if snd else None
parent = None
# check whether skipping or a parent body preference has been
# specified
if sbd and tbd:
sbdid = sbd.id()
tbdid = tbd.id()
# check whether there is an entry in skip_prefs directing that
# this loop constraint conversion should be skipped
if self.skip_prefs.get(sbdid, -1) == tbdid:
return False
if self.skip_prefs.get(tbdid, -1) == sbdid:
return False
# check whether a parent body preference has been registered
if self.parent_prefs.get(sbdid, -1) == tbdid:
parent = tbd
if self.verbose:
print(f"Found parent preference {tbd.name()} for {sbd.name()}")
elif self.parent_prefs.get(tbdid, -1) == sbdid:
parent = sbd
if self.verbose:
print(f"Found parent preference {sbd.name()} for {tbd.name()}")
# if no parent has been found, auto-detect
if not parent:
if sbd in basebds:
parent = sbd
if tbd in basebds:
parent = tbd
if parent:
if self.verbose:
print(
" HAVE PARENT",
parent.name(),
sbd.name(),
sbd.parentHinge().hingeType(),
tbd.name(),
tbd.parentHinge().hingeType(),
)
# the parent body has been specified
if sbd and sbd.id() == parent.id():
# the source body is the parent, so can convert if the
# target body has a 6 dof hinge
return tbd and tbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF
else:
# the source body is not the parent
assert tbd
assert tbd.id() == parent.id()
# can convert if the source body has a 6 dof hinge
return sbd and sbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF
assert 0
if self.verbose:
print(
" NO PARENT",
sbd.name(),
sbd.parentHinge().hingeType(),
tbd.name(),
tbd.parentHinge().hingeType(),
)
# no parent body has been specified, so just need one of the source
# or target bodies to have a 6 dof hinge
if sbd and sbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF:
return True
if tbd and tbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF:
return True
return False
[docs]
def convertToHinge(self, lc: LoopConstraintHinge, basebds: Container[PhysicalBody]):
"""
Convert the loop constraint into a hinge and return the new child body.
"""
if self.verbose:
print(f" Converting constraint {lc.name()}/{lc.hinge().hingeType()}")
snd = lc.sourceNode()
tnd = lc.targetNode()
sbd = snd.parentBody() if snd else None
tbd = tnd.parentBody() if snd else None
if self.verbose and sbd:
print(f" source bd={sbd.name()}/{sbd.parentHinge().hingeType()}")
if self.verbose and tnd:
print(f" target bd={tbd.name()}/{sbd.parentHinge().hingeType()}")
parent = None
# figure out the parent if none has been specified
if not parent:
if sbd in basebds:
parent = sbd
if tbd in basebds:
parent = tbd
else:
# the specified parent should be either sbd or tbd
assert (sbd.id() == parent.id()) or (tbd.id() == parent.id())
if self.verbose and parent:
print(f" parent={parent.name()}")
# figure out the orientation of the hinge
found_orientation = False
if parent:
# the parent body has been specified. The polarity will depend
# on whether this body is the parent of the source or the target
# node
if sbd:
if sbd.id() == parent.id():
reverse = False
found_orientation = True
if not found_orientation:
# the source node did not work out, check the target node
assert tbd
assert tbd.id() == parent.id()
reverse = True
found_orientation = True
else:
# no parent body, so the hinge polarity has to be auto-determined
# based on which of the nodes has a 6 dof hinge
if sbd:
if not tbd:
# no target body, so the source has to be the parent
assert sbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF
reverse = False
found_orientation = True
else:
# we do have a target body, and need to make sure that
# the polarity is towards a body with the 6dof hinge
if tbd.parentHinge().hingeType() != HINGE_TYPE.FULL6DOF:
# the target body does not have a 6dof hinge, so the
# target has to be the parent body
reverse = True
found_orientation = True
else:
# the target does has a 6dof hinge
reverse = sbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF
found_orientation = True
else:
# no source body, so the target has to be the parent body
assert tbd
# assert (tbd.parentHinge().hingeType() == HINGE_TYPE.FULL6DOF)
reverse = True
found_orientation = True
if self.verbose:
print(f" reverse={reverse}")
hge = LoopConstraintHinge.toPhysicalHinge(lc, reverse)
self.donelcs.append(lc.id())
if self.verbose:
print(
" Created hinge:",
hge.onode().parentBody().name(),
" --> ",
hge.pnode().parentBody().name(),
hge.hingeType(),
)
if snd:
del snd
if tnd:
del tnd
if sbd:
del sbd
if tbd:
del tbd
del parent
# bodies.remove(hge.onode().parentBody())
parid = hge.onode().parentBody().id()
if parid not in self.donebds:
self.donebds.append(parid)
return hge.pnode().parentBody()
[docs]
def convertBranch(self, bd: PhysicalBody, basebds: Container[PhysicalBody]):
"""Convert this bd's loop constraints to hinges, and continue down
the branch to do this for downstream bodies as well. If bd is in the
basebds list, then use it as the parent body.
"""
bdid = bd.id()
if bdid in self.donebds:
return
multibody = bd.multibody()
lcs = [x for x in multibody.getBodyLoopConstraints(bd) if x.hasHinge()]
if self.verbose:
print(" Doing ", bd.name(), "lcs=", len(lcs))
if lcs:
children = []
for k in range(len(lcs)):
if not self.canConvertToHinge(lcs[k], basebds):
if self.verbose:
print(f" Skipping conversion of {lcs[k].name()} loop constraint")
continue
child_body = self.convertToHinge(lcs[k], basebds)
discard(lcs[k])
lcs[k] = None
children.append(child_body)
for child in children:
self.convertBranch(child, basebds)
else:
if bdid not in self.skipbds:
self.skipbds.append(bdid)
[docs]
def convertBranchBF(self, basebds: Container[PhysicalBody]):
"""Convert this bd's loop constraints to hinges, and continue down
the branch to do this for downstream bodies as well. If bd is in the
basebds list, then use it as the parent body. Breadth first
traverse.
"""
bd = self.visited.pop()
if bd in self.todo:
self.todo.remove(bd)
multibody = bd.multibody()
lcs = [x for x in multibody.getBodyLoopConstraints(bd) if x.hasHinge()]
if self.verbose:
print(" Doing ", bd.name(), "lcs=", len(lcs))
if lcs:
children = []
for k in range(len(lcs)):
if not self.canConvertToHinge(lcs[k], basebds):
if self.verbose:
print(f" Skipping conversion of {lcs[k].name()} loop constraint")
continue
child_body = self.convertToHinge(lcs[k], basebds)
discard(lcs[k])
lcs[k] = None
self.visited.append(child_body)
"""
FIXME: can this orphaned string be deleted?
- create visited queue and put basebodies in it
- while queue not empty, call convert2BranchBF
- this will process the first entry in the queue, and add all children to teh back
"""
