diff --git a/src/adam/model/mj_factories/mj_joint.py b/src/adam/model/mj_factories/mj_joint.py index 0e34e88..f517457 100644 --- a/src/adam/model/mj_factories/mj_joint.py +++ b/src/adam/model/mj_factories/mj_joint.py @@ -35,82 +35,3 @@ def __init__( self.origin = Pose(xyz=joint_ori_xyz, rpy=joint_rpy) self.limit = joint["limit"] self.idx = idx - - def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: - """ - Args: - q (npt.ArrayLike): joint value - - Returns: - npt.ArrayLike: the homogenous transform of a joint, given q - """ - - if self.type == "fixed": - xyz = self.origin.xyz - rpy = self.origin.rpy - return self.math.H_from_Pos_RPY(xyz, rpy) - elif self.type in ["revolute", "continuous"]: - return self.math.H_revolute_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - elif self.type in ["prismatic"]: - return self.math.H_prismatic_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - - def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: - """ - Args: - q (npt.ArrayLike): joint motion - - Returns: - npt.ArrayLike: spatial transform of the joint given q - """ - if self.type == "fixed": - return self.math.X_fixed_joint(self.origin.xyz, self.origin.rpy) - elif self.type in ["revolute", "continuous"]: - return self.math.X_revolute_joint( - self.origin.xyz, self.origin.rpy, self.axis, q - ) - elif self.type in ["prismatic"]: - return self.math.X_prismatic_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - - def motion_subspace(self) -> npt.ArrayLike: - """ - Args: - joint (Joint): Joint - - Returns: - npt.ArrayLike: motion subspace of the joint - """ - if self.type == "fixed": - return self.math.vertcat(0, 0, 0, 0, 0, 0) - elif self.type in ["revolute", "continuous"]: - return self.math.vertcat( - 0, - 0, - 0, - self.axis[0], - self.axis[1], - self.axis[2], - ) - elif self.type in ["prismatic"]: - return self.math.vertcat( - self.axis[0], - self.axis[1], - self.axis[2], - 0, - 0, - 0, - ) diff --git a/src/adam/model/mj_factories/mj_link.py b/src/adam/model/mj_factories/mj_link.py index dd38139..b6f6c7b 100644 --- a/src/adam/model/mj_factories/mj_link.py +++ b/src/adam/model/mj_factories/mj_link.py @@ -63,25 +63,3 @@ def transform_inertia_tensor( """Transform the diagonal inertia tensor to the original frame.""" I_diag = np.diag(diaginertia) return rotation_matrix @ I_diag @ rotation_matrix.T - - def spatial_inertia(self) -> npt.ArrayLike: - """ - Returns: - npt.ArrayLike: the 6x6 inertia matrix expressed at - the origin of the link (with rotation) - """ - I = self.inertial.inertia - mass = self.inertial.mass - o = self.inertial.origin.xyz - rpy = self.inertial.origin.rpy - return self.math.spatial_inertia(I, mass, o, rpy) - - def homogeneous(self) -> npt.ArrayLike: - """ - Returns: - npt.ArrayLike: the homogeneous transform of the link - """ - return self.math.H_from_Pos_RPY( - self.inertial.origin.xyz, - self.inertial.origin.rpy, - ) diff --git a/src/adam/model/std_factories/std_joint.py b/src/adam/model/std_factories/std_joint.py index a72787d..330f5f2 100644 --- a/src/adam/model/std_factories/std_joint.py +++ b/src/adam/model/std_factories/std_joint.py @@ -25,82 +25,3 @@ def __init__( self.origin = joint.origin self.limit = joint.limit self.idx = idx - - def homogeneous(self, q: npt.ArrayLike) -> npt.ArrayLike: - """ - Args: - q (npt.ArrayLike): joint value - - Returns: - npt.ArrayLike: the homogenous transform of a joint, given q - """ - - if self.type == "fixed": - xyz = self.origin.xyz - rpy = self.origin.rpy - return self.math.H_from_Pos_RPY(xyz, rpy) - elif self.type in ["revolute", "continuous"]: - return self.math.H_revolute_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - elif self.type in ["prismatic"]: - return self.math.H_prismatic_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - - def spatial_transform(self, q: npt.ArrayLike) -> npt.ArrayLike: - """ - Args: - q (npt.ArrayLike): joint motion - - Returns: - npt.ArrayLike: spatial transform of the joint given q - """ - if self.type == "fixed": - return self.math.X_fixed_joint(self.origin.xyz, self.origin.rpy) - elif self.type in ["revolute", "continuous"]: - return self.math.X_revolute_joint( - self.origin.xyz, self.origin.rpy, self.axis, q - ) - elif self.type in ["prismatic"]: - return self.math.X_prismatic_joint( - self.origin.xyz, - self.origin.rpy, - self.axis, - q, - ) - - def motion_subspace(self) -> npt.ArrayLike: - """ - Args: - joint (Joint): Joint - - Returns: - npt.ArrayLike: motion subspace of the joint - """ - if self.type == "fixed": - return self.math.vertcat(0, 0, 0, 0, 0, 0) - elif self.type in ["revolute", "continuous"]: - return self.math.vertcat( - 0, - 0, - 0, - self.axis[0], - self.axis[1], - self.axis[2], - ) - elif self.type in ["prismatic"]: - return self.math.vertcat( - self.axis[0], - self.axis[1], - self.axis[2], - 0, - 0, - 0, - ) diff --git a/src/adam/model/std_factories/std_link.py b/src/adam/model/std_factories/std_link.py index 7754747..e4af585 100644 --- a/src/adam/model/std_factories/std_link.py +++ b/src/adam/model/std_factories/std_link.py @@ -2,7 +2,7 @@ import urdf_parser_py.urdf from adam.core.spatial_math import SpatialMath -from adam.model import Link, Inertial, Pose +from adam.model import Inertial, Link, Pose class StdLink(Link): @@ -24,25 +24,3 @@ def __init__(self, link: urdf_parser_py.urdf.Link, math: SpatialMath): link.inertial.origin = Pose(xyz=[0, 0, 0], rpy=[0, 0, 0]) self.inertial = link.inertial - - def spatial_inertia(self) -> npt.ArrayLike: - """ - Returns: - npt.ArrayLike: the 6x6 inertia matrix expressed at - the origin of the link (with rotation) - """ - I = self.inertial.inertia - mass = self.inertial.mass - o = self.inertial.origin.xyz - rpy = self.inertial.origin.rpy - return self.math.spatial_inertia(I, mass, o, rpy) - - def homogeneous(self) -> npt.ArrayLike: - """ - Returns: - npt.ArrayLike: the homogeneous transform of the link - """ - return self.math.H_from_Pos_RPY( - self.inertial.origin.xyz, - self.inertial.origin.rpy, - )