Skip to content

Commit

Permalink
Refactor: Use private rotation and translation attributes in Coordina…
Browse files Browse the repository at this point in the history 
…tes methods
  • Loading branch information
@iory
iory committed Nov 3, 2023
1 parent 0fb43f2 commit a52e79c
Showing 1 changed file with 58 additions and 57 deletions.
115 changes: 58 additions & 57 deletions skrobot/coordinates/base.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ def transform_coords(c1, c2, out=None):
out = Coordinates(check_validity=False)
elif not isinstance(out, Coordinates):
raise TypeError("Input type should be skrobot.coordinates.Coordinates")
out._translation = c1.translation + np.dot(c1._rotation, c2._translation)
out._translation = c1._translation + np.dot(c1._rotation, c2._translation)
out._rotation = np.matmul(c1._rotation, c2._rotation)
return out

Expand Down Expand Up @@ -380,7 +380,7 @@ def dimension(self):
len(self.translation) : int
dimension of this coordinate
"""
return len(self.translation)
return len(self._translation)

@property
def x_axis(self):
Expand All @@ -391,7 +391,7 @@ def x_axis(self):
axis : numpy.ndarray
x axis.
"""
return np.array(self.rotation[:, 0].T, 'f')
return np.array(self._rotation[:, 0].T, 'f')

@property
def y_axis(self):
Expand All @@ -402,7 +402,7 @@ def y_axis(self):
axis : numpy.ndarray
y axis.
"""
return np.array(self.rotation[:, 1].T, 'f')
return np.array(self._rotation[:, 1].T, 'f')

@property
def z_axis(self):
Expand All @@ -413,7 +413,7 @@ def z_axis(self):
axis : numpy.ndarray
z axis.
"""
return np.array(self.rotation[:, 2].T, 'f')
return np.array(self._rotation[:, 2].T, 'f')

def changed(self):
"""Return False
Expand Down Expand Up @@ -467,8 +467,8 @@ def translate(self, vec, wrt='local'):
"""
vec = np.array(vec, dtype=np.float64)
return self.newcoords(
self.rotation,
self.parent_orientation(vec, wrt) + self.translation,
self._rotation,
self.parent_orientation(vec, wrt) + self._translation,
check_validity=False)

def transform_vector(self, v):
Expand All @@ -489,9 +489,9 @@ def transform_vector(self, v):
"""
v = np.array(v, dtype=np.float64)
if v.ndim == 2:
return (np.matmul(self.rotation, v.T)
+ self.translation.reshape(3, -1)).T
return np.matmul(self.rotation, v) + self.translation
return (np.matmul(self._rotation, v.T)
+ self._translation.reshape(3, -1)).T
return np.matmul(self._rotation, v) + self._translation

def inverse_transform_vector(self, vec):
"""Transform vector in world coordinates to local coordinates
Expand All @@ -508,11 +508,11 @@ def inverse_transform_vector(self, vec):
"""
vec = np.array(vec, dtype=np.float64)
if vec.ndim == 2:
return (np.matmul(self.rotation.T, vec.T)
return (np.matmul(self._rotation.T, vec.T)
- np.matmul(
self.rotation.T, self.translation).reshape(3, -1)).T
return np.matmul(self.rotation.T, vec) - \
np.matmul(self.rotation.T, self.translation)
self._rotation.T, self._translation).reshape(3, -1)).T
return np.matmul(self._rotation.T, vec) - \
np.matmul(self._rotation.T, self._translation)

def inverse_transformation(self, dest=None):
"""Return a invese transformation of this coordinate system.
Expand Down Expand Up @@ -541,9 +541,9 @@ def inverse_transformation(self, dest=None):
"""
if dest is None:
dest = Coordinates(check_validity=False)
dest.rotation = self.rotation.T
dest.translation = np.matmul(dest.rotation, self.translation)
dest.translation = -1.0 * dest.translation
dest._rotation = self._rotation.T
dest._translation = np.matmul(dest._rotation, self._translation)
dest._translation = -1.0 * dest._translation
return dest

def transformation(self, c2, wrt='local'):
Expand Down Expand Up @@ -596,8 +596,8 @@ def T(self):
"""
matrix = np.zeros((4, 4), dtype=np.float64)
matrix[3, 3] = 1.0
matrix[:3, :3] = self.rotation
matrix[:3, 3] = self.translation
matrix[:3, :3] = self._rotation
matrix[:3, 3] = self._translation
return matrix

@property
Expand Down Expand Up @@ -634,13 +634,13 @@ def dual_quaternion(self):
DualQuaternion representation of this coordinate
"""
qr = normalize_vector(self.quaternion)
x, y, z = self.translation
x, y, z = self._translation
qd = quaternion_multiply(np.array([0, x, y, z]), qr) * 0.5
return DualQuaternion(qr, qd)

def parent_orientation(self, v, wrt):
if wrt == 'local' or wrt == self:
return np.matmul(self.rotation, v)
return np.matmul(self._rotation, v)
if wrt == 'parent' \
or wrt == self.parent \
or wrt == 'world':
Expand All @@ -659,7 +659,7 @@ def rotate_vector(self, v):
Returns
-------
np.matmul(self.rotation, v) : numpy.ndarray
np.matmul(self._rotation, v) : numpy.ndarray
rotated vector
Examples
Expand Down Expand Up @@ -745,7 +745,7 @@ def rpy_angle(self):
Returns
-------
rpy_angle(self.rotation) : tuple(numpy.ndarray, numpy.ndarray)
rpy_angle(self._rotation) : tuple(numpy.ndarray, numpy.ndarray)
a pair of rpy angles. See also skrobot.coordinates.math.rpy_angle
Examples
Expand All @@ -757,7 +757,7 @@ def rpy_angle(self):
(array([ 3.84592537e-16, -1.04719755e+00, 1.57079633e+00]),
array([ 3.14159265, -2.0943951 , -1.57079633]))
"""
return rpy_angle(self.rotation)
return rpy_angle(self._rotation)

def axis(self, ax):
ax = convert_to_axis_vector(ax)
Expand Down Expand Up @@ -941,19 +941,19 @@ def rotate_with_matrix(self, mat, wrt='local'):
self : skrobot.coordinates.Coordinates
"""
if wrt == 'local' or wrt == self:
rot = np.matmul(self.rotation, mat)
self.newcoords(rot, self.translation, check_validity=False)
rot = np.matmul(self._rotation, mat)
self.newcoords(rot, self._translation, check_validity=False)
elif wrt == 'parent' or wrt == self.parent or \
wrt == 'world' or wrt is None or \
wrt == worldcoords:
rot = np.matmul(mat, self.rotation)
self.newcoords(rot, self.translation, check_validity=False)
rot = np.matmul(mat, self._rotation)
self.newcoords(rot, self._translation, check_validity=False)
elif isinstance(wrt, Coordinates):
r2 = wrt.worldrot()
r2t = r2.T
r2t = np.matmul(mat, r2t)
r2t = np.matmul(r2, r2t)
self._rotation = np.matmul(r2t, self.rotation)
self._rotation = np.matmul(r2t, self._rotation)
else:
raise ValueError('wrt {} is not supported'.format(wrt))
return self
Expand Down Expand Up @@ -1005,11 +1005,11 @@ def rotate(self, theta, axis=None, wrt='local', skip_normalization=False):
self.rotate_with_matrix(theta, wrt)
elif wrt == 'local' or wrt == self:
self._rotation = rotate_matrix(
self.rotation, theta, axis,
self._rotation, theta, axis,
skip_normalization=skip_normalization)
elif wrt == 'parent' or wrt == 'world':
self._rotation = rotate_matrix(
self.rotation, theta,
self._rotation, theta,
axis, True,
skip_normalization=skip_normalization)
elif isinstance(wrt, Coordinates): # C1'=C2*R*C2(-1)*C1
Expand All @@ -1018,7 +1018,7 @@ def rotate(self, theta, axis=None, wrt='local', skip_normalization=False):
skip_normalization=skip_normalization), wrt)
else:
raise ValueError('wrt {} not supported'.format(wrt))
return self.newcoords(self.rotation, self.translation,
return self.newcoords(self._rotation, self._translation,
check_validity=False)

def orient_with_matrix(self, rotation_matrix, wrt='world'):
Expand Down Expand Up @@ -1072,10 +1072,10 @@ def worldrot(self):
Returns
-------
self.rotation : numpy.ndarray
self._rotation : numpy.ndarray
rotation matrix of this coordinate
"""
return self.rotation
return self._rotation

def worldpos(self):
"""Return translation of this coordinate
Expand All @@ -1087,7 +1087,7 @@ def worldpos(self):
self.translation : numpy.ndarray
translation of this coordinate
"""
return self.translation
return self._translation

def newcoords(self, c, pos=None, check_validity=True):
"""Update of coords is always done through newcoords.
Expand Down Expand Up @@ -1174,7 +1174,7 @@ def __repr__(self):
def __str__(self):
self.worldrot()
pos = self.worldpos()
self.rpy = rpy_angle(self.rotation)[0]
self.rpy = rpy_angle(self._rotation)[0]
if self.name:
prefix = self.__class__.__name__ + ':' + self.name
else:
Expand All @@ -1201,7 +1201,7 @@ def __init__(self, parent=None, *args, **kwargs):
self._descendants = []

self._worldcoords = Coordinates(pos=self.translation,
rot=self.rotation,
rot=self._rotation,
hook=self.update,
check_validity=False)

Expand Down Expand Up @@ -1288,7 +1288,7 @@ def assoc(self, child, relative_coords='world', force=False,
.format(child.parent.name)
raise RuntimeError(msg)

if not (child in self.descendants):
if not (child in self._descendants):
if relative_coords is None or relative_coords == 'world':
relative_coords = self.worldcoords().transformation(
child.worldcoords())
Expand All @@ -1305,7 +1305,7 @@ def assoc(self, child, relative_coords='world', force=False,
return child

def dissoc(self, child):
if child in self.descendants:
if child in self._descendants:
c = child.worldcoords().copy_coords()
self._descendants.remove(child)
child.parent = None
Expand Down Expand Up @@ -1335,7 +1335,7 @@ def newcoords(self, c, pos=None, check_validity=True):
def changed(self):
if self._changed is False:
self._changed = True
return [c.changed() for c in self.descendants]
return [c.changed() for c in self._descendants]
return [False]

def parentcoords(self):
Expand All @@ -1351,24 +1351,24 @@ def inverse_transform_vector(self, v):

def rotate_with_matrix(self, matrix, wrt):
if wrt == 'local' or wrt == self:
self._rotation = np.dot(self.rotation, matrix)
return self.newcoords(self.rotation, self.translation,
self._rotation = np.dot(self._rotation, matrix)
return self.newcoords(self._rotation, self._translation,
check_validity=False)
elif wrt == 'parent' or wrt == self.parent:
rotation = np.matmul(matrix, self.rotation)
rotation = np.matmul(matrix, self._rotation)
return self.newcoords(
rotation, self.translation, check_validity=False)
rotation, self._translation, check_validity=False)
else:
parent_coords = self.parentcoords()
parent_rot = parent_coords.rotation
parent_rot = parent_coords._rotation
if isinstance(wrt, Coordinates):
wrt_rot = wrt.worldrot()
matrix = np.matmul(wrt_rot, matrix)
matrix = np.matmul(matrix, wrt_rot.T)
matrix = np.matmul(matrix, parent_rot)
matrix = np.matmul(parent_rot.T, matrix)
rotation = np.matmul(matrix, self.rotation)
return self.newcoords(rotation, self.translation,
rotation = np.matmul(matrix, self._rotation)
return self.newcoords(rotation, self._translation,
check_validity=False)

def rotate(self, theta, axis, wrt='local', skip_normalization=False):
Expand Down Expand Up @@ -1399,14 +1399,14 @@ def rotate(self, theta, axis, wrt='local', skip_normalization=False):
return self.rotate_with_matrix(theta, wrt)

if wrt == 'local' or wrt == self:
rotation = rotate_matrix(self.rotation, theta, axis,
rotation = rotate_matrix(self._rotation, theta, axis,
skip_normalization=skip_normalization)
return self.newcoords(rotation, self.translation,
return self.newcoords(rotation, self._translation,
check_validity=False)
elif wrt == 'parent' or wrt == self.parent:
rotation = rotate_matrix(self.rotation, theta, axis,
rotation = rotate_matrix(self._rotation, theta, axis,
skip_normalization=skip_normalization)
return self.newcoords(rotation, self.translation,
return self.newcoords(rotation, self._translation,
check_validity=False)
else:
return self.rotate_with_matrix(
Expand All @@ -1432,18 +1432,19 @@ def orient_with_matrix(self, rotation_matrix, wrt='world'):
# R_{input} = R_{world} = R_{parent} R_{this}
# R_{this} = R_{parent}^{-1} R_{input}
parent_worldcoords = self.parentcoords()
rotation = parent_worldcoords.rotation.T.dot(rotation_matrix)
rotation = parent_worldcoords._rotation.T.dot(rotation_matrix)
elif isinstance(wrt, Coordinates):
# R_{world} = R_{wrt} R_{input}
# R_{world} = R_{parent} R_{this}
# R_{this} = R_{parent}^{-1} R_{world}
# R_{this} = R_{parent}^{-1} R_{world} R_{wrt} R_{input}
world_rotation_matrix = wrt.worldrot().dot(rotation_matrix)
parent_worldcoords = self.parentcoords()
rotation = parent_worldcoords.rotation.T.dot(world_rotation_matrix)
rotation = parent_worldcoords._rotation.T.dot(
world_rotation_matrix)
else:
raise TypeError('wrt {} not supported'.format(wrt))
return self.newcoords(rotation, self.translation,
return self.newcoords(rotation, self._translation,
check_validity=False)

def rotate_vector(self, v):
Expand Down Expand Up @@ -1497,7 +1498,7 @@ def transform(self, c, wrt='local', out=None):
out, out)
else:
raise ValueError('transform wrt {} is not supported'.format(wrt))
return out.newcoords(out.rotation, out.translation,
return out.newcoords(out._rotation, out._translation,
check_validity=False)

def update(self, force=False):
Expand All @@ -1524,10 +1525,10 @@ def worldcoords(self):
return self._worldcoords

def worldrot(self):
return self.worldcoords().rotation
return self.worldcoords()._rotation

def worldpos(self):
return self.worldcoords().translation
return self.worldcoords()._translation

@property
def parent(self):
Expand Down

0 comments on commit a52e79c

Please sign in to comment.