From a52e79c736f4b4f358ba6c83d5ff7858497c5a63 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 3 Nov 2023 13:55:08 +0900 Subject: [PATCH] Refactor: Use private rotation and translation attributes in Coordinates methods --- skrobot/coordinates/base.py | 115 ++++++++++++++++++------------------ 1 file changed, 58 insertions(+), 57 deletions(-) diff --git a/skrobot/coordinates/base.py b/skrobot/coordinates/base.py index f26bd68a..d8fd5e59 100644 --- a/skrobot/coordinates/base.py +++ b/skrobot/coordinates/base.py @@ -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 @@ -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): @@ -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): @@ -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): @@ -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 @@ -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): @@ -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 @@ -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. @@ -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'): @@ -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 @@ -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': @@ -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 @@ -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 @@ -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) @@ -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 @@ -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 @@ -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'): @@ -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 @@ -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. @@ -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: @@ -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) @@ -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()) @@ -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 @@ -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): @@ -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): @@ -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( @@ -1432,7 +1432,7 @@ 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} @@ -1440,10 +1440,11 @@ def orient_with_matrix(self, rotation_matrix, wrt='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): @@ -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): @@ -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):