# 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.
from abc import abstractmethod
from typing import TYPE_CHECKING, cast, Self, Annotated
from pydantic import SerializeAsAny
import numpy as np
import Karana.Scene as ks
from Karana.Math.Ktyping import (
Vec3,
Vec,
normCheck,
npSizeCheck,
)
import Karana.Math as km
from Karana.KUtils.DataStruct import (
DataStruct,
IdMixin,
NestedBaseMixin,
)
from Karana.KUtils._toPythonScript import (
valuePrinter,
fullPrintOpts,
)
if TYPE_CHECKING:
from Karana.Dynamics import (
PhysicalBody,
HingeType,
PhysicalSubhinge,
SubhingeType,
LockedSubhinge,
PinSubhinge,
LinearSubhinge,
Linear3Subhinge,
SphericalSubhinge,
SphericalQuatSubhinge,
ScrewSubhinge,
PhysicalHinge,
FramePairHinge,
ParentPhysicalBodyAttachmentParams,
ParentBodyAttachmentParams,
)
else:
from ._SOADyn_Py import (
PhysicalBody,
HingeType,
PhysicalSubhinge,
SubhingeType,
LockedSubhinge,
PinSubhinge,
LinearSubhinge,
Linear3Subhinge,
SphericalSubhinge,
SphericalQuatSubhinge,
ScrewSubhinge,
PhysicalHinge,
FramePairHinge,
ParentPhysicalBodyAttachmentParams,
ParentBodyAttachmentParams,
)
SubhingePyType = (
LockedSubhinge
| PinSubhinge
| LinearSubhinge
| Linear3Subhinge
| ScrewSubhinge
| SphericalSubhinge
| SphericalQuatSubhinge
)
class PhysicalSubhingeParams(DataStruct, NestedBaseMixin, IdMixin[PhysicalSubhinge]):
"""Base class for all subhinges.
Parameters
----------
subhinge_type: SubhingeType
The subhinge type associated with the subhinge.
"""
subhinge_type: SubhingeType
[docs]
@classmethod
def fromSubhinge(cls, sh: SubhingePyType) -> "PhysicalSubhingeParams":
"""Create an instance of PhysicalSubhingeParams from the provided Subhinge.
Parameters
----------
sh : SubhingeType
The Subhinge used to create the PhysicalSubhingeParams.
Returns
-------
Self
A PhysicalSubhingeParams that represents the provided Subhinge.
"""
if isinstance(sh, PinSubhinge):
return PinSubhingeParams.fromSubhinge(sh)
elif isinstance(sh, LockedSubhinge):
return LockedPhysicalSubhingeParams.fromSubhinge(sh)
elif isinstance(sh, LinearSubhinge):
return LinearSubhingeParams.fromSubhinge(sh)
elif isinstance(sh, SphericalSubhinge):
return SphericalSubhingeParams.fromSubhinge(sh)
elif isinstance(sh, SphericalQuatSubhinge):
return SphericalQuatSubhingeParams.fromSubhinge(sh)
elif isinstance(sh, Linear3Subhinge):
return Linear3SubhingeParams.fromSubhinge(sh)
else: # Screw subhinge
sh = cast(ScrewSubhinge, sh)
return ScrewSubhingeParams.fromSubhinge(sh)
[docs]
@abstractmethod
def setSubhingeValues(self, sh: SubhingePyType) -> None:
"""Set the subhinge values for the provided subhinge."""
...
@abstractmethod
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool, /
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values."""
...
class PhysicalHingeParams(DataStruct):
"""Base class for all hinge types.
Hinges that have 0 axes associated with them, e.g., BALL,
use this DataStruct. Classes with more axes (or information) will use derived classes.
Parameters
----------
hinge_type : HingeType
The hinge type associated with the hinge.
subhinge_params: list[PhysicalSubhingeParams]
A list of subhinge params that make up this hinge.
"""
hinge_type: HingeType
subhinge_params: list[SerializeAsAny[PhysicalSubhingeParams]]
[docs]
def toHinge(self, parent_bd: PhysicalBody, child_bd: PhysicalBody) -> PhysicalHinge:
"""Create a Hinge between the provided bodies.
Parameters
----------
parent_bd : PhysicalBody
The parent body for the hinge.
child_bd : PhysicalBody
The child body for the hinge.
Returns
-------
PhysicalHinge
The hinge that was created between the two provided bodies.
"""
if self.hinge_type == HingeType.CUSTOM:
hge = PhysicalHinge(
parent_bd,
child_bd,
HingeType.CUSTOM,
[x.subhinge_type for x in self.subhinge_params],
)
else:
hge = PhysicalHinge(parent_bd, child_bd, self.hinge_type)
for sh, sh_ds in zip(
cast(list[PhysicalSubhinge], [hge.subhinge(k) for k in range(hge.nSubhinges())]),
self.subhinge_params,
):
sh_ds.addObjectFromId(sh)
sh_ds.setSubhingeValues(sh) # pyright: ignore
return hge
[docs]
@classmethod
def fromHinge(cls, hge: FramePairHinge, htype: HingeType) -> Self:
"""Create a PhysicalHingeParams from the provided hinge.
Parameters
----------
hge : FramePairHinge
The FramePairHinge used to create the PhysicalHingeParams.
htype : HingeType
The hinge type associated with the FramePairHinge.
Returns
-------
Self
A PhysicalHingeParams that represents the provided FramePairHinge.
"""
return cls(
hinge_type=htype,
subhinge_params=cast(
list[PhysicalSubhingeParams],
[
PhysicalSubhingeParams.fromSubhinge(cast(SubhingePyType, hge.subhinge(k)))
for k in range(hge.nSubhinges())
],
),
)
class PhysicalBodyParams(DataStruct, NestedBaseMixin):
"""Parameters used to configure a PhysicalBody.
Parameters
----------
spatial_inertia : Karana.Math.SpatialInertia
Spatial inertia assigned to the body.
body_to_joint_transform : Karana.Math.HomTran
Transform from the body frame to its hinge joint frame.
hinge_params : PhysicalHingeParams
Parameters for the body's physical hinge.
parent_body_attachment_params : ParentBodyAttachmentParams
Parameters for attaching this body to its parent.
scene_part_specs : list[ScenePartSpec]
Scene-part specifications attached to the body.
scene_file_object_specs : list[SceneFileObjectSpec]
Scene-file-object specifications attached to the body.
"""
spatial_inertia: km.SpatialInertia
body_to_joint_transform: km.HomTran
hinge_params: PhysicalHingeParams
parent_body_attachment_params: ParentBodyAttachmentParams
scene_part_specs: list[ks.ScenePartSpec]
scene_file_object_specs: list[ks.SceneFileObjectSpec]
[docs]
@classmethod
def createDefault(cls) -> Self:
"""Create a version of PhysicalBodyParams with all values filled in.
This is useful for testing or when you just need a body for a simple demo.
Returns
-------
Self
An instance of PhysicalBodyParams with all values filled in.
"""
return cls(
spatial_inertia=km.SpatialInertia(
2.0, np.array([0.3, 0.2, 0.1]), np.diag([3.0, 2.0, 1.0])
),
body_to_joint_transform=km.HomTran(
km.UnitQuaternion(0.8, 0.6, 0.0, 0.0), np.array([0.2, 0.3, 0.4])
),
hinge_params=PhysicalHingeParams(
hinge_type=HingeType.REVOLUTE,
subhinge_params=[PinSubhingeParams(unit_axis=np.array([0.0, 1.0, 0.0]))],
),
parent_body_attachment_params=ParentPhysicalBodyAttachmentParams(
inb_to_joint_transform=km.HomTran(
km.UnitQuaternion(0.5, 0.5, 0.5, 0.5), np.array([0.4, 0.2, 0.3])
)
),
scene_part_specs=[],
scene_file_object_specs=[],
)
class PinSubhingeParams(PhysicalSubhingeParams):
"""PinSubhinge DataStruct.
Parameters
----------
unit_axis : Vec3
Unit axis for the pin subhinge.
joint_limits: Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
prescribed : bool
Determines if the subhinge is prescribed or not.
"""
subhinge_type: SubhingeType = SubhingeType.PIN
unit_axis: Annotated[Vec3, normCheck(1.0)]
joint_limits: Annotated[Vec, npSizeCheck((2,))] = np.array([km.notReadyNaN, km.notReadyNaN])
prescribed: bool = False
[docs]
@classmethod
def fromSubhinge(cls, sh: PinSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a PinSubhingeParams from a PinSubhinge."""
joint_limits = sh.getJointLimits()
dark = cls(
subhinge_type=SubhingeType.PIN,
unit_axis=sh.getUnitAxis(),
prescribed=sh.getPrescribed(),
joint_limits=joint_limits,
)
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, sh: PinSubhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a PinSubhinge."""
sh.setUnitAxis(self.unit_axis)
sh.setPrescribed(self.prescribed)
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
sh.setJointLimits(self.joint_limits)
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool, /
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have PinSubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
imports = {"import numpy as np"}
code = ""
with fullPrintOpts():
if params_only:
imports.add("import Karana.Dynamics as kd")
code += f"""kd.PinSubhingeParams(
unit_axis={valuePrinter(self.unit_axis)},
prescribed={self.prescribed},
"""
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
code += f"joint_limits={valuePrinter(self.joint_limits)}\n"
code += ")\n"
else:
code += f"{sh_var_name}.setUnitAxis({valuePrinter(self.unit_axis)})\n"
if self.prescribed:
code += f"{sh_var_name}.setPrescribed({self.prescribed})\n"
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
imports.add("import numpy as np")
code += f"{sh_var_name}.setJointLimits({valuePrinter(self.joint_limits)})\n"
return imports, code
class LinearSubhingeParams(PhysicalSubhingeParams):
"""LinearSubhinge DataStruct.
Parameters
----------
unit_axis : Vec3
Unit axis for the pin subhinge.
joint_limits: Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
prescribed : bool
Determines if the subhinge is prescribed or not.
"""
subhinge_type: SubhingeType = SubhingeType.LINEAR
unit_axis: Annotated[Vec3, normCheck(1.0)]
joint_limits: Annotated[Vec, npSizeCheck((2,))] = np.array([km.notReadyNaN, km.notReadyNaN])
prescribed: bool = False
[docs]
@classmethod
def fromSubhinge(cls, sh: LinearSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a LinearSubhingeParams from a LinearSubhinge."""
joint_limits = sh.getJointLimits()
dark = cls(
unit_axis=sh.getUnitAxis(), prescribed=sh.getPrescribed(), joint_limits=joint_limits
)
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, sh: LinearSubhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a LinearSubhinge."""
sh.setUnitAxis(self.unit_axis)
sh.setPrescribed(self.prescribed)
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
sh.setJointLimits(self.joint_limits)
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool, /
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have LinearSubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
imports = {"import numpy as np"}
code = ""
with fullPrintOpts():
if params_only:
imports.add("import Karana.Dynamics as kd")
code += f"""kd.LinearSubhingeParams(
unit_axis={valuePrinter(self.unit_axis)},
prescribed={self.prescribed},
"""
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
code += f"joint_limits={valuePrinter(self.joint_limits)}\n"
code += ")\n"
else:
code += f"{sh_var_name}.setUnitAxis({valuePrinter(self.unit_axis)})\n"
if self.prescribed:
code += f"{sh_var_name}.setPrescribed({self.prescribed})\n"
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
imports.add("import numpy as np")
code += f"{sh_var_name}.setJointLimits({valuePrinter(self.joint_limits)})\n"
return imports, code
class SphericalSubhingeParams(PhysicalSubhingeParams):
"""SphericalSubhinge DataStruct.
prescribed : bool
Determines if the subhinge is prescribed or not.
"""
subhinge_type: SubhingeType = SubhingeType.SPHERICAL
prescribed: bool = False
[docs]
@classmethod
def fromSubhinge(cls, sh: SphericalSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a SphericalSubhingeParams from a SphericalSubhinge."""
dark = cls(prescribed=sh.getPrescribed())
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, sh: SphericalSubhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a SphericalSubhinge."""
sh.setPrescribed(self.prescribed)
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have SphericalSubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
if params_only:
return (
{"import Karana.Dynamics as kd"},
f"""kd.SphericalSubhingeParams(
prescribed={self.prescribed}
)
""",
)
else:
if self.prescribed:
return set(), f"{sh_var_name}.setPrescribed({self.prescribed})\n"
else:
return set(), ""
class SphericalQuatSubhingeParams(PhysicalSubhingeParams):
"""SphericalQuatSubhinge DataStruct.
prescribed : bool
Determines if the subhinge is prescribed or not.
"""
subhinge_type: SubhingeType = SubhingeType.SPHERICAL_QUAT
prescribed: bool = False
[docs]
@classmethod
def fromSubhinge( # pyright: ignore - override is okay.
cls, sh: SphericalQuatSubhinge
) -> Self:
"""Create a SphericalQuatSubhingeParams from a SphericalQuatSubhinge."""
dark = cls(prescribed=sh.getPrescribed())
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues( # pyright: ignore - override is okay.
self, sh: SphericalQuatSubhinge
) -> None: # pyright: ignore - override is okay.
"""Set the values of a SphericalQuatSubhinge."""
sh.setPrescribed(self.prescribed)
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have SphericalQuatSubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
if params_only:
return (
{"import Karana.Dynamics as kd"},
f"kd.SphericalQuatSubhingeParams(prescribed={self.prescribed})\n",
)
if self.prescribed:
return set(), f"{sh_var_name}.setPrescribed({self.prescribed})\n"
return set(), ""
class Linear3SubhingeParams(PhysicalSubhingeParams):
"""Linear3Subhinge DataStruct.
Parameters
----------
prescribed : bool
Determines if the subhinge is prescribed or not.
"""
subhinge_type: SubhingeType = SubhingeType.LINEAR3
prescribed: bool = False
[docs]
@classmethod
def fromSubhinge(cls, sh: Linear3Subhinge) -> Self: # pyright: ignore - override is okay.
"""Create a Linear3SubhingeParams from a Linear3Subhinge."""
dark = cls(prescribed=sh.getPrescribed())
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, sh: Linear3Subhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a Linear3Subhinge."""
sh.setPrescribed(self.prescribed)
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have Linear3SubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
if params_only:
return (
{"import Karana.Dynamics as kd"},
f"""kd.Linear3SubhingeParams(
prescribed={self.prescribed}
)
""",
)
else:
if self.prescribed:
return set(), f"{sh_var_name}.setPrescribed({self.prescribed})\n"
else:
return set(), ""
class LockedPhysicalSubhingeParams(PhysicalSubhingeParams):
"""LockedSubhinge DataStruct."""
subhinge_type: SubhingeType = SubhingeType.LOCKED
[docs]
@classmethod
def fromSubhinge(cls, sh: LockedSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a LockedPhysicalSubhingeParams from a LockedSubhinge."""
dark = cls()
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, _: LockedSubhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a LockedSubhinge."""
pass
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have LockedSubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
if params_only:
return {"import Karana.Dynamics as kd"}, "kd.LockedPhysicalSubhingeParams()\n"
else:
return set(), ""
class ScrewSubhingeParams(PhysicalSubhingeParams):
"""ScrewSubhinge DataStruct.
Parameters
----------
prescribed : bool
Determines if the subhinge is prescribed or not.
unit_axis : Vec3
Unit axis for the pin subhinge.
pitch : float
The pitch of the screw subhinge.
joint_limits: Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
"""
subhinge_type: SubhingeType = SubhingeType.SCREW
prescribed: bool = False
unit_axis: Annotated[Vec3, normCheck(1.0)]
pitch: float
joint_limits: Annotated[Vec, npSizeCheck((2,))] = np.array([km.notReadyNaN, km.notReadyNaN])
[docs]
@classmethod
def fromSubhinge(cls, sh: ScrewSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a ScrewSubhingeParams from a ScrewSubhinge."""
joint_limits = sh.getJointLimits()
dark = cls(
unit_axis=sh.getUnitAxis(),
pitch=sh.getPitch(),
prescribed=sh.getPrescribed(),
joint_limits=joint_limits,
)
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, sh: ScrewSubhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a ScrewSubhinge."""
sh.setUnitAxis(self.unit_axis)
sh.setPitch(self.pitch)
sh.setPrescribed(self.prescribed)
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
sh.setJointLimits(self.joint_limits)
def _setPythonScriptSubhingeValues(
self, sh_var_name: str, params_only: bool, /
) -> tuple[set[str], str]:
"""Create a string for setting the subhinge values.
Parameters
----------
sh_var_name : str
The variable name for the subhinge.
params_only : bool
Code will only have ScrewSubhingeParams rather than API calls
to set values.
Returns
-------
tuple[set[str], str]
* The set of imports needed
* The code to add to the Python script
"""
imports = {"import numpy as np"}
code = ""
if params_only:
imports.add("import Karana.Dynamics as kd")
code += f"""kd.ScrewSubhingeParams(
unit_axis={valuePrinter(self.unit_axis)},
pitch={valuePrinter(self.pitch)},
prescribed={self.prescribed}
)
"""
else:
with fullPrintOpts():
code += f"{sh_var_name}.setUnitAxis({valuePrinter(self.unit_axis)})\n"
code += f"{sh_var_name}.setPitch({valuePrinter(self.unit_axis)})\n"
if self.prescribed:
code += f"{sh_var_name}.setPrescribed({self.prescribed})\n"
if not all(km.isNotReadyNaN(value) for value in self.joint_limits):
imports.add("import numpy as np")
code += f"{sh_var_name}.setJointLimits({valuePrinter(self.joint_limits)})\n"
return imports, code
__all__ = [
"PhysicalBodyParams",
"PhysicalSubhingeParams",
"PhysicalHingeParams",
"PinSubhingeParams",
"LinearSubhingeParams",
"SphericalSubhingeParams",
"SphericalQuatSubhingeParams",
"Linear3SubhingeParams",
"LockedPhysicalSubhingeParams",
"ScrewSubhingeParams",
"SubhingePyType",
]
