Skip to content

Commit

Permalink
Remove function implementation since I moved them into the correspond…
Browse files Browse the repository at this point in the history 
…ing abc classes
  • Loading branch information
@Giulero
Giulero committed Jul 17, 2024
1 parent 18e7cce commit 21fe057
Show file tree
Hide file tree
Showing 4 changed files with 1 addition and 203 deletions.
79 changes: 0 additions & 79 deletions src/adam/model/mj_factories/mj_joint.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
)
22 changes: 0 additions & 22 deletions src/adam/model/mj_factories/mj_link.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
)
79 changes: 0 additions & 79 deletions src/adam/model/std_factories/std_joint.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
)
24 changes: 1 addition & 23 deletions src/adam/model/std_factories/std_link.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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,
)

0 comments on commit 21fe057

Please sign in to comment.