# 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.Kquantities import ureg
from Karana.Math.Ktyping import (
Vec3,
Vec,
Mat,
normCheck,
npSizeCheck,
Length,
Angle,
)
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,
LockedOffsetSubhinge,
PinSubhinge,
LinearSubhinge,
Linear3Subhinge,
SphericalSubhinge,
SphericalQuatSubhinge,
ScrewSubhinge,
PhysicalHinge,
FramePairHinge,
ParentPhysicalBodyAttachmentParams,
ParentBodyAttachmentParams,
)
else:
from ._SOADyn_pybind11_Py import (
PhysicalBody,
HingeType,
PhysicalSubhinge,
SubhingeType,
LockedSubhinge,
LockedOffsetSubhinge,
PinSubhinge,
LinearSubhinge,
Linear3Subhinge,
SphericalSubhinge,
SphericalQuatSubhinge,
ScrewSubhinge,
PhysicalHinge,
FramePairHinge,
ParentPhysicalBodyAttachmentParams,
ParentBodyAttachmentParams,
)
SubhingePyType = (
LockedSubhinge
| LockedOffsetSubhinge
| 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, LockedOffsetSubhinge):
return LockedOffsetPhysicalSubhingeParams.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)
if sh_ds._id is None:
sh_ds._id = sh.id()
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.
range_limits: Annotated[Vec, npSizeCheck((1,2))]
The lower and upper bound for the Q coordinate range limits.
"""
subhinge_type: SubhingeType = SubhingeType.PIN
unit_axis: Annotated[Vec3, normCheck(1.0)]
joint_limits: Angle[Annotated[Vec, npSizeCheck((2,))]] = (
np.array([km.notReadyNaN, km.notReadyNaN]) * ureg.rad
)
prescribed: bool = False
range_limits: Angle[Annotated[Mat, npSizeCheck((1, 2))]] = (
np.full((1, 2), km.notReadyNaN) * ureg.rad
)
if TYPE_CHECKING:
def __init__(
self,
*,
unit_axis: Vec3,
joint_limits: Angle[Annotated[Vec, npSizeCheck((2,))]]
| Annotated[Vec, npSizeCheck((2,))] = np.array([km.notReadyNaN, km.notReadyNaN])
* ureg.rad,
prescribed: bool = False,
range_limits: Angle[Annotated[Mat, npSizeCheck((1, 2))]]
| Annotated[Mat, npSizeCheck((1, 2))] = np.full((1, 2), km.notReadyNaN) * ureg.rad,
):
"""PinSubhinge DataStruct.
Parameters
----------
unit_axis : Vec3
Unit axis for the pin subhinge.
joint_limits: Angle[Annotated[Vec, npSizeCheck((2,))]] | Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
prescribed : bool
Determines if the subhinge is prescribed or not.
range_limits: Angle[Annotated[Mat, npSizeCheck((1,2))]] | Annotated[Mat, npSizeCheck((1,2))]]
The lower and upper bound for the Q coordinate range limits.
"""
...
[docs]
@classmethod
def fromSubhinge(cls, sh: PinSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a PinSubhingeParams from a PinSubhinge."""
joint_limits = sh.getJointLimits()
range_limits = sh.getRangeLimits()
dark = cls(
unit_axis=sh.getUnitAxis(),
prescribed=sh.getPrescribed(),
joint_limits=joint_limits,
range_limits=range_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)
allReadyVec = np.vectorize(km.isNotReadyNaN)
if not np.all(allReadyVec(self.joint_limits.m)):
sh.setJointLimits(self.joint_limits)
if not np.all(allReadyVec(self.range_limits.m)):
sh.setRangeLimits(self.range_limits[0], 0)
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.m):
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.m):
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.
range_limits: Annotated[Vec, npSizeCheck((1,2))]
The lower and upper bound for the Q coordinate range limits.
"""
subhinge_type: SubhingeType = SubhingeType.LINEAR
unit_axis: Annotated[Vec3, normCheck(1.0)]
joint_limits: Length[Annotated[Vec, npSizeCheck((2,))]] = (
np.array([km.notReadyNaN, km.notReadyNaN]) * ureg.m
)
prescribed: bool = False
range_limits: Length[Annotated[Mat, npSizeCheck((1, 2))]] = (
np.full((1, 2), km.notReadyNaN) * ureg.m
)
if TYPE_CHECKING:
def __init__(
self,
*,
unit_axis: Vec3,
joint_limits: Length[Annotated[Vec, npSizeCheck((2,))]]
| Annotated[Vec, npSizeCheck((2,))] = np.array([km.notReadyNaN, km.notReadyNaN])
* ureg.m,
prescribed: bool = False,
range_limits: Length[Annotated[Mat, npSizeCheck((1, 2))]]
| Annotated[Mat, npSizeCheck((1, 2))] = np.full((1, 2), km.notReadyNaN) * ureg.m,
):
"""LinearSubhinge DataStruct.
Parameters
----------
unit_axis : Vec3
Unit axis for the pin subhinge.
joint_limits: Length[Annotated[Vec, npSizeCheck((2,))]] | Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
prescribed : bool
Determines if the subhinge is prescribed or not.
range_limits: Length[Annotated[Mat, npSizeCheck((1,2))]] | Annotated[Mat, npSizeCheck((1,2))]]
The lower and upper bound for the Q coordinate range limits.
"""
...
[docs]
@classmethod
def fromSubhinge(cls, sh: LinearSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a LinearSubhingeParams from a LinearSubhinge."""
joint_limits = sh.getJointLimits()
range_limits = sh.getRangeLimits()
dark = cls(
unit_axis=sh.getUnitAxis(),
prescribed=sh.getPrescribed(),
joint_limits=joint_limits,
range_limits=range_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)
allReadyVec = np.vectorize(km.isNotReadyNaN)
if not np.all(allReadyVec(self.joint_limits.m)):
sh.setJointLimits(self.joint_limits)
if not np.all(allReadyVec(self.range_limits.m)):
sh.setRangeLimits(self.range_limits[0], 0)
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.m):
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.m):
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.
range_limits: Annotated[Vec, npSizeCheck((3,2))]
The lower and upper bound for the Q coordinate range limits.
"""
subhinge_type: SubhingeType = SubhingeType.SPHERICAL
prescribed: bool = False
range_limits: Annotated[Mat, npSizeCheck((3, 2))] = np.full((3, 2), km.notReadyNaN)
[docs]
@classmethod
def fromSubhinge(cls, sh: SphericalSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a SphericalSubhingeParams from a SphericalSubhinge."""
range_limits = sh.getRangeLimits()
dark = cls(prescribed=sh.getPrescribed(), range_limits=range_limits)
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)
allReadyVec = np.vectorize(km.isNotReadyNaN)
if not np.all(allReadyVec(self.range_limits)):
for i in range(0, 3):
sh.setRangeLimits(self.range_limits[i], i)
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.
range_limits: Annotated[Vec, npSizeCheck((3,2))]
The lower and upper bound for the Q coordinate range limits.
"""
subhinge_type: SubhingeType = SubhingeType.SPHERICAL_QUAT
prescribed: bool = False
range_limits: Annotated[Mat, npSizeCheck((4, 2))] = np.full((4, 2), km.notReadyNaN)
[docs]
@classmethod
def fromSubhinge( # pyright: ignore - override is okay.
cls, sh: SphericalQuatSubhinge
) -> Self:
"""Create a SphericalQuatSubhingeParams from a SphericalQuatSubhinge."""
range_limits = sh.getRangeLimits()
dark = cls(prescribed=sh.getPrescribed(), range_limits=range_limits)
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)
allReadyVec = np.vectorize(km.isNotReadyNaN)
if not np.all(allReadyVec(self.range_limits)):
for i in range(0, 4):
sh.setRangeLimits(self.range_limits[i], i)
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.
range_limits: Annotated[Vec, npSizeCheck((3,2))]
The lower and upper bound for the Q coordinate range limits.
"""
subhinge_type: SubhingeType = SubhingeType.LINEAR3
prescribed: bool = False
range_limits: Length[Annotated[Mat, npSizeCheck((3, 2))]] = (
np.full((3, 2), km.notReadyNaN) * ureg.m
)
if TYPE_CHECKING:
def __init__(
self,
*,
subhinge_type: SubhingeType = SubhingeType.LINEAR3,
prescribed: bool = False,
range_limits: Length[Annotated[Mat, npSizeCheck((3, 2))]]
| Annotated[Mat, npSizeCheck((3, 2))] = np.full((3, 2), km.notReadyNaN) * ureg.m,
):
"""Linear3Subhinge DataStruct.
Parameters
----------
prescribed : bool
Determines if the subhinge is prescribed or not.
range_limits: Length[Annotated[Vec, npSizeCheck((3,2))]] | Annotated[Vec, npSizeCheck((3,2))]
The lower and upper bound for the Q coordinate range limits.
"""
...
[docs]
@classmethod
def fromSubhinge(cls, sh: Linear3Subhinge) -> Self: # pyright: ignore - override is okay.
"""Create a Linear3SubhingeParams from a Linear3Subhinge."""
range_limits = sh.getRangeLimits()
dark = cls(prescribed=sh.getPrescribed(), range_limits=range_limits)
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)
allReadyVec = np.vectorize(km.isNotReadyNaN)
if not np.all(allReadyVec(self.range_limits.m)):
for i in range(0, 3):
sh.setRangeLimits(self.range_limits[i], i)
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 LockedOffsetPhysicalSubhingeParams(PhysicalSubhingeParams):
"""LockedOffsetSubhinge DataStruct.
Parameters
----------
offset : HomTran
The offset transform
"""
subhinge_type: SubhingeType = SubhingeType.LOCKED_OFFSET
offset: km.HomTran
if TYPE_CHECKING:
def __init__(self, *, offset: km.HomTran):
"""LockedOffsetSubhinge DataStruct.
Parameters
----------
offset : HomTran
The transform offset
"""
...
[docs]
@classmethod
def fromSubhinge(cls, sh: LockedOffsetSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a LockedOffsetPhysicalSubhingeParams from a LockedOffsetSubhinge."""
offset = sh.relTransform()
dark = cls(offset=offset)
dark._id = sh.id()
cls.addObjectFromId(dark, sh)
return dark
[docs]
def setSubhingeValues(self, sh: LockedOffsetSubhinge) -> None: # pyright: ignore - override is okay.
"""Set the values of a LockedOffsetSubhinge."""
sh.setOffset(self.offset)
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 LockedOffsetSubhingeParams 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.LockedOffsetPhysicalSubhingeParams(offset={valuePrinter(self.offset)})\n"
else:
# return set(), ""
with fullPrintOpts():
code += f"{sh_var_name}.setOffset({valuePrinter(self.offset)})\n"
return imports, code
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 : Length[float]
The pitch of the screw subhinge.
joint_limits: Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
range_limits: Annotated[Vec, npSizeCheck((1,2))]
The lower and upper bound for the Q coordinate range limits.
"""
subhinge_type: SubhingeType = SubhingeType.SCREW
prescribed: bool = False
unit_axis: Annotated[Vec3, normCheck(1.0)]
pitch: Length[float]
joint_limits: Angle[Annotated[Vec, npSizeCheck((2,))]] = (
np.array([km.notReadyNaN, km.notReadyNaN]) * ureg.rad
)
range_limits: Angle[Annotated[Mat, npSizeCheck((1, 2))]] = (
np.full((1, 2), km.notReadyNaN) * ureg.rad
)
if TYPE_CHECKING:
def __init__(
self,
*,
unit_axis: Vec3,
prescribed: bool = False,
pitch: Length[float] | float,
joint_limits: Angle[Annotated[Vec, npSizeCheck((2,))]]
| Annotated[Vec, npSizeCheck((2,))] = np.array([km.notReadyNaN, km.notReadyNaN])
* ureg.rad,
range_limits: Angle[Annotated[Mat, npSizeCheck((1, 2))]]
| Annotated[Mat, npSizeCheck((1, 2))] = np.full((1, 2), km.notReadyNaN) * ureg.rad,
):
"""ScrewSubhinge DataStruct.
Parameters
----------
prescribed : bool
Determines if the subhinge is prescribed or not.
unit_axis : Vec3
Unit axis for the pin subhinge.
pitch : Length[float] | float
The pitch of the screw subhinge.
joint_limits: Angle[Annotated[Vec, npSizeCheck((2,))]] | Annotated[Vec, npSizeCheck((2,))]
The lower and upper bound for the joint limit.
range_limits: Angle[Annotated[Vec, npSizeCheck((1,2))]] | Annotated[Vec, npSizeCheck((1,2))]
The lower and upper bound for the Q coordinate range limits.
"""
...
[docs]
@classmethod
def fromSubhinge(cls, sh: ScrewSubhinge) -> Self: # pyright: ignore - override is okay.
"""Create a ScrewSubhingeParams from a ScrewSubhinge."""
joint_limits = sh.getJointLimits()
range_limits = sh.getRangeLimits()
dark = cls(
unit_axis=sh.getUnitAxis(),
pitch=sh.getPitch(),
prescribed=sh.getPrescribed(),
joint_limits=joint_limits,
range_limits=range_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)
allReadyVec = np.vectorize(km.isNotReadyNaN)
if not np.all(allReadyVec(self.joint_limits.m)):
sh.setJointLimits(self.joint_limits)
if not np.all(allReadyVec(self.range_limits.m)):
sh.setRangeLimits(self.range_limits[0], 0)
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.m):
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",
"LockedOffsetPhysicalSubhingeParams",
"ScrewSubhingeParams",
"SubhingePyType",
]
