import abc
import dataclasses
import numpy.typing as npt
from adam.core.spatial_math import SpatialMath
@dataclasses.dataclass(frozen=True, slots=True)
[docs]
class Pose:
"""Pose class"""
@staticmethod
[docs]
def build(xyz: npt.ArrayLike, rpy: npt.ArrayLike, math: SpatialMath) -> "Pose":
xyz = math.asarray(xyz)
rpy = math.asarray(rpy)
return Pose(xyz, rpy)
@staticmethod
[docs]
def zero(math: SpatialMath) -> "Pose":
return Pose.build([0, 0, 0], [0, 0, 0], math)
[docs]
def get_xyz(self) -> npt.ArrayLike:
return self.xyz
[docs]
def get_rpy(self) -> npt.ArrayLike:
return self.rpy
@dataclasses.dataclass(frozen=True, slots=True)
[docs]
class Inertia:
@staticmethod
[docs]
def build(
ixx: npt.DTypeLike,
ixy: npt.DTypeLike,
ixz: npt.DTypeLike,
iyy: npt.DTypeLike,
iyz: npt.DTypeLike,
izz: npt.DTypeLike,
math: SpatialMath,
) -> "Inertia":
matrix = math.asarray(
[
[ixx, ixy, ixz],
[ixy, iyy, iyz],
[ixz, iyz, izz],
]
)
return Inertia(matrix, ixx, ixy, ixz, iyy, iyz, izz)
@staticmethod
[docs]
def zero(math: SpatialMath) -> "Inertia":
return Inertia.build(
ixx=0.0, ixy=0.0, ixz=0.0, iyy=0.0, iyz=0.0, izz=0.0, math=math
)
[docs]
def get_matrix(self) -> npt.ArrayLike:
return self.matrix
@dataclasses.dataclass(frozen=True, slots=True)
[docs]
class Limits:
"""Limits class"""
[docs]
velocity: npt.ArrayLike
@dataclasses.dataclass
[docs]
class Joint(abc.ABC):
"""Base Joint class. You need to fill at least these fields"""
"""
Abstract base class for all joints.
"""
@abc.abstractmethod
@abc.abstractmethod
[docs]
def motion_subspace(self) -> npt.ArrayLike:
"""
Returns:
npt.ArrayLike: motion subspace of the joint
"""
@abc.abstractmethod
[docs]
def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike:
"""
Args:
q (npt.ArrayLike): joint value
Returns:
npt.ArrayLike: homogeneous transform given the joint value
"""
pass
@dataclasses.dataclass(frozen=True, slots=True)
[docs]
class Inertial:
"""Inertial description"""
@staticmethod
[docs]
def zero(math: SpatialMath) -> "Inertial":
"""Returns an Inertial object with zero mass and inertia"""
xyz = math.factory.zeros(3)
rpy = math.factory.zeros(3)
zero_element = math.factory.zeros(1)
return Inertial(
mass=zero_element,
inertia=Inertia.zero(math),
origin=Pose(xyz=xyz, rpy=rpy),
)
[docs]
def set_mass(self, mass: npt.ArrayLike) -> "Inertial":
"""Set the mass of the inertial object"""
self.mass = mass
[docs]
def set_inertia(self, inertia: Inertia) -> "Inertial":
"""Set the inertia of the inertial object"""
self.inertia = inertia
[docs]
def set_origin(self, origin: Pose) -> "Inertial":
"""Set the origin of the inertial object"""
self.origin = origin
@dataclasses.dataclass
[docs]
class Link(abc.ABC):
"""Base Link class. You need to fill at least these fields"""
@abc.abstractmethod
[docs]
def spatial_inertia(self) -> npt.ArrayLike:
"""
Returns:
npt.ArrayLike: the 6x6 inertia matrix expressed at
the origin of the link (with rotation)
"""
pass
@abc.abstractmethod
[docs]
def homogeneous(self) -> npt.ArrayLike:
"""
Returns:
npt.ArrayLike: the homogeneous transform of the link
"""
pass
@dataclasses.dataclass
[docs]
class ModelFactory(abc.ABC):
"""The abstract class of the model factory.
The model factory is responsible for creating the model.
You need to implement all the methods in your concrete implementation
"""
@abc.abstractmethod
[docs]
def build_link(self) -> Link:
"""build the single link
Returns:
Link
"""
pass
@abc.abstractmethod
[docs]
def build_joint(self) -> Joint:
"""build the single joint
Returns:
Joint
"""
pass
@abc.abstractmethod
[docs]
def get_links(self) -> list[Link]:
"""
Returns:
list[Link]: the list of the link
"""
pass
@abc.abstractmethod
[docs]
def get_frames(self) -> list[Link]:
"""
Returns:
list[Link]: the list of the frames
"""
pass
@abc.abstractmethod
[docs]
def get_joints(self) -> list[Joint]:
"""
Returns:
list[Joint]: the list of the joints
"""
pass