From fe67d3bac5b5529de16d386ce28fcfe0843fe795 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 2 Nov 2023 20:43:39 +0900 Subject: [PATCH 1/5] Optimized Coordinates assignment to prevent unnecessary deepcopy --- skrobot/coordinates/base.py | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/skrobot/coordinates/base.py b/skrobot/coordinates/base.py index 489a162d..76ba50e2 100644 --- a/skrobot/coordinates/base.py +++ b/skrobot/coordinates/base.py @@ -1100,18 +1100,26 @@ def newcoords(self, c, pos=None, check_validity=True): """ if pos is not None: if check_validity: - self.rotation = copy.deepcopy(c) - self.translation = copy.deepcopy(pos) + if id(self._rotation) != id(c): + self.rotation = copy.deepcopy(c) + if id(self._translation) != id(pos): + self.translation = copy.deepcopy(pos) else: - self._rotation = np.copy(c) - self._translation = np.copy(pos) + if id(self._rotation) != id(c): + self._rotation = np.copy(c) + if id(self._translation) != id(pos): + self._translation = np.copy(pos) else: if check_validity: - self.rotation = copy.deepcopy(c.rotation) - self.translation = copy.deepcopy(c.translation) + if id(self._rotation) != id(c._rotation): + self.rotation = copy.deepcopy(c._rotation) + if id(self._translation) != id(c._translation): + self.translation = copy.deepcopy(c._translation) else: - self._rotation = np.copy(c.rotation) - self._translation = np.copy(c.translation) + if id(self._rotation) != id(c._rotation): + self._rotation = np.copy(c._rotation) + if id(self._translation) != id(c._translation): + self._translation = np.copy(c._translation) return self def __mul__(self, other_c): From c1af090485468404f87c2132bc2a3e3e3202dfb7 Mon Sep 17 00:00:00 2001 From: iory Date: Thu, 2 Nov 2023 21:08:14 +0900 Subject: [PATCH 2/5] Rename _wrap_axis to convert_to_axis_vector --- docs/source/reference/functions.rst | 2 +- skrobot/coordinates/__init__.py | 2 +- skrobot/coordinates/base.py | 6 +- skrobot/coordinates/geo.py | 6 +- skrobot/coordinates/math.py | 95 ++++++++++++++++------------- skrobot/model/joint.py | 6 +- skrobot/model/robot_model.py | 6 +- 7 files changed, 65 insertions(+), 58 deletions(-) diff --git a/docs/source/reference/functions.rst b/docs/source/reference/functions.rst index c9064d61..bf65fbda 100644 --- a/docs/source/reference/functions.rst +++ b/docs/source/reference/functions.rst @@ -10,7 +10,7 @@ Utilities functions :toctree: generated/ :nosignatures: - skrobot.coordinates.math._wrap_axis + skrobot.coordinates.math.convert_to_axis_vector skrobot.coordinates.math._check_valid_rotation skrobot.coordinates.math._check_valid_translation skrobot.coordinates.math.triple_product diff --git a/skrobot/coordinates/__init__.py b/skrobot/coordinates/__init__.py index 340c98a4..68d88a69 100644 --- a/skrobot/coordinates/__init__.py +++ b/skrobot/coordinates/__init__.py @@ -6,7 +6,7 @@ from .base import make_coords from .base import make_cascoords -from .geo import _wrap_axis +from .geo import convert_to_axis_vector from .geo import midcoords from .geo import midpoint from .geo import orient_coords_to_axis diff --git a/skrobot/coordinates/base.py b/skrobot/coordinates/base.py index 76ba50e2..112a596e 100644 --- a/skrobot/coordinates/base.py +++ b/skrobot/coordinates/base.py @@ -8,8 +8,8 @@ from skrobot.coordinates.dual_quaternion import DualQuaternion from skrobot.coordinates.math import _check_valid_rotation from skrobot.coordinates.math import _check_valid_translation -from skrobot.coordinates.math import _wrap_axis from skrobot.coordinates.math import angle_between_vectors +from skrobot.coordinates.math import convert_to_axis_vector from skrobot.coordinates.math import cross_product from skrobot.coordinates.math import matrix2quaternion from skrobot.coordinates.math import matrix_log @@ -763,7 +763,7 @@ def rpy_angle(self): return rpy_angle(self.rotation) def axis(self, ax): - ax = _wrap_axis(ax) + ax = convert_to_axis_vector(ax) return self.rotate_vector(ax) def difference_position(self, coords, @@ -802,7 +802,7 @@ def difference_position(self, coords, array([ 0.2, -0.5, -0.2]) """ dif_pos = self.inverse_transform_vector(coords.worldpos()) - translation_axis = _wrap_axis(translation_axis) + translation_axis = convert_to_axis_vector(translation_axis) dif_pos[translation_axis == 1] = 0.0 return dif_pos diff --git a/skrobot/coordinates/geo.py b/skrobot/coordinates/geo.py index 427d792e..1f779551 100644 --- a/skrobot/coordinates/geo.py +++ b/skrobot/coordinates/geo.py @@ -1,8 +1,8 @@ import numpy as np from skrobot.coordinates import make_coords -from skrobot.coordinates.math import _wrap_axis from skrobot.coordinates.math import angle_between_vectors +from skrobot.coordinates.math import convert_to_axis_vector from skrobot.coordinates.math import midpoint from skrobot.coordinates.math import midrot from skrobot.coordinates.math import normalize_vector @@ -50,7 +50,7 @@ def orient_coords_to_axis(target_coords, v, axis='z', eps=0.005): v : list or numpy.ndarray position of target [x, y, z] axis : list or string or numpy.ndarray - see _wrap_axis function + see convert_to_axis_vector function eps : float (optional) eps @@ -91,7 +91,7 @@ def orient_coords_to_axis(target_coords, v, axis='z', eps=0.005): if np.linalg.norm(v) == 0.0: v = np.array([0, 0, 1], 'f') nv = normalize_vector(v) - axis = _wrap_axis(axis) + axis = convert_to_axis_vector(axis) ax = target_coords.rotate_vector(axis) rot_axis = np.cross(ax, nv) rot_angle_cos = np.clip(np.dot(nv, ax), -1.0, 1.0) diff --git a/skrobot/coordinates/math.py b/skrobot/coordinates/math.py index 609e7d92..ab61432f 100644 --- a/skrobot/coordinates/math.py +++ b/skrobot/coordinates/math.py @@ -7,15 +7,30 @@ from math import cos from math import pi from math import sin +import warnings import numpy as np # epsilon for testing whether a number is close to zero _EPS = np.finfo(float).eps * 4.0 - - -def _wrap_axis(axis): +_AXIS_VECTORS = { + 'x': np.array([1, 0, 0]), + 'y': np.array([0, 1, 0]), + 'z': np.array([0, 0, 1]), + '-x': np.array([-1, 0, 0]), + '-y': np.array([0, -1, 0]), + '-z': np.array([0, 0, -1]), + 'xy': np.array([1, 1, 0]), + 'yx': np.array([1, 1, 0]), + 'yz': np.array([0, 1, 1]), + 'zy': np.array([0, 1, 1]), + 'zx': np.array([1, 0, 1]), + 'xz': np.array([1, 0, 1]), +} + + +def convert_to_axis_vector(axis): """Convert axis to float vector. Parameters @@ -30,60 +45,52 @@ def _wrap_axis(axis): Examples -------- - >>> from skrobot.coordinates.math import _wrap_axis - >>> _wrap_axis('x') + >>> from skrobot.coordinates.math import convert_to_axis_vector + >>> convert_to_axis_vector('x') array([1, 0, 0]) - >>> _wrap_axis('y') + >>> convert_to_axis_vector('y') array([0, 1, 0]) - >>> _wrap_axis('z') + >>> convert_to_axis_vector('z') array([0, 0, 1]) - >>> _wrap_axis('xy') + >>> convert_to_axis_vector('xy') array([1, 1, 0]) - >>> _wrap_axis([1, 1, 1]) + >>> convert_to_axis_vector([1, 1, 1]) array([1, 1, 1]) - >>> _wrap_axis(True) + >>> convert_to_axis_vector(True) array([0, 0, 0]) - >>> _wrap_axis(False) + >>> convert_to_axis_vector(False) array([1, 1, 1]) """ if isinstance(axis, str): - if axis in ['x', 'xx']: - axis = np.array([1, 0, 0]) - elif axis in ['y', 'yy']: - axis = np.array([0, 1, 0]) - elif axis in ['z', 'zz']: - axis = np.array([0, 0, 1]) - elif axis == '-x': - axis = np.array([-1, 0, 0]) - elif axis == '-y': - axis = np.array([0, -1, 0]) - elif axis == '-z': - axis = np.array([0, 0, -1]) - elif axis in ['xy', 'yx']: - axis = np.array([1, 1, 0]) - elif axis in ['yz', 'zy']: - axis = np.array([0, 1, 1]) - elif axis in ['zx', 'xz']: - axis = np.array([1, 0, 1]) - else: - raise NotImplementedError + try: + return _AXIS_VECTORS[axis] + except KeyError: + raise NotImplementedError( + "Axis conversion for '{}' is not implemented.".format(axis)) elif isinstance(axis, list): - if not len(axis) == 3: - raise ValueError - axis = np.array(axis) + if len(axis) != 3: + raise ValueError("Axis list must have exactly three elements.") + return np.array(axis) elif isinstance(axis, np.ndarray): - if not axis.shape == (3,): - raise ValueError + if axis.shape != (3,): + raise ValueError("Axis ndarray must be of shape (3,).") + return axis elif isinstance(axis, bool): - if axis is True: - return np.array([0, 0, 0]) - else: - return np.array([1, 1, 1]) + # If True, returns a zero vector; if False, returns a ones vector + return np.zeros(3) if axis else np.ones(3) elif axis is None: - return np.array([1, 1, 1]) + return np.ones(3) else: - raise ValueError - return axis + raise ValueError("Invalid type for axis. " + "Must be one of: str, list, ndarray, bool, None.") + + +def _wrap_axis(axis): + warnings.warn( + 'Function `_wrap_axis` is deprecated. ' + 'Please use `convert_to_axis_vector` instead', + DeprecationWarning) + return convert_to_axis_vector(axis) def _check_valid_rotation(rotation): @@ -413,7 +420,7 @@ def rotation_matrix(theta, axis): [ 0.00000000e+00, 1.00000000e+00, 0.00000000e+00], [-1.00000000e+00, 0.00000000e+00, 2.22044605e-16]]) """ - axis = _wrap_axis(axis) + axis = convert_to_axis_vector(axis) axis = axis / np.sqrt(np.dot(axis, axis)) a = np.cos(theta / 2.0) b, c, d = -axis * np.sin(theta / 2.0) diff --git a/skrobot/model/joint.py b/skrobot/model/joint.py index f8ac392d..ff08bb6c 100644 --- a/skrobot/model/joint.py +++ b/skrobot/model/joint.py @@ -2,7 +2,7 @@ import numpy as np -from skrobot.coordinates import _wrap_axis +from skrobot.coordinates import convert_to_axis_vector from skrobot.coordinates.math import cross_product from skrobot.coordinates import normalize_vector @@ -191,7 +191,7 @@ def __init__(self, axis='z', max_joint_velocity=max_joint_velocity, max_joint_torque=max_joint_torque, *args, **kwargs) - self.axis = normalize_vector(_wrap_axis(axis)) + self.axis = normalize_vector(convert_to_axis_vector(axis)) self._joint_angle = 0.0 if self.min_angle is None: @@ -343,7 +343,7 @@ def __init__(self, axis='z', max_joint_velocity=np.pi / 4, # [m/s] max_joint_torque=100, # [N] *args, **kwargs): - self.axis = normalize_vector(_wrap_axis(axis)) + self.axis = normalize_vector(convert_to_axis_vector(axis)) self._joint_angle = 0.0 super(LinearJoint, self).__init__( max_joint_velocity=max_joint_velocity, diff --git a/skrobot/model/robot_model.py b/skrobot/model/robot_model.py index 9eb8b0f5..aa1d377e 100644 --- a/skrobot/model/robot_model.py +++ b/skrobot/model/robot_model.py @@ -9,8 +9,8 @@ import six import trimesh -from skrobot.coordinates import _wrap_axis from skrobot.coordinates import CascadedCoords +from skrobot.coordinates import convert_to_axis_vector from skrobot.coordinates import Coordinates from skrobot.coordinates import make_coords from skrobot.coordinates import make_matrix @@ -1060,7 +1060,7 @@ def ik_convergence_check( translation_axis : list of axis rotation_axis : list of axis - see _wrap_axis + see convert_to_axis_vector """ for i in range(len(dif_pos)): if LA.norm(dif_pos[i]) > thre[i]: @@ -1532,7 +1532,7 @@ def calc_jacobian_from_link_list(self, if self._is_relevant(joint, move_target): if joint.joint_dof <= 1: - paxis = _wrap_axis(joint.axis) + paxis = convert_to_axis_vector(joint.axis) else: paxis = joint.axis child_link = joint.child_link From 361b132d2dd88e2349db1e359d737692d899b150 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 3 Nov 2023 13:07:42 +0900 Subject: [PATCH 3/5] Optimize update method by avoiding context manager when hook is None --- skrobot/coordinates/base.py | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/skrobot/coordinates/base.py b/skrobot/coordinates/base.py index 112a596e..d29c0674 100644 --- a/skrobot/coordinates/base.py +++ b/skrobot/coordinates/base.py @@ -1,4 +1,3 @@ -import contextlib import copy import sys import warnings @@ -227,14 +226,12 @@ def __init__(self, self.parent = None self._hook = hook - @contextlib.contextmanager def disable_hook(self): - hook = self._hook - self._hook = None - try: - yield - finally: - self._hook = hook + if self._hook is not None: + original_hook = self._hook + self._hook = None + return True, original_hook + return False, None def get_transform(self): """Return Transform object @@ -1492,15 +1489,19 @@ def transform(self, c, wrt='local', out=None): def update(self, force=False): if not force and not self._changed: return - with self.disable_hook(): - if self.parent: + hook_disabled, original_hook = self.disable_hook() + try: + if self._parent: transform_coords( - self.parent.worldcoords(), + self._parent.worldcoords(), self, self._worldcoords) else: - self._worldcoords._rotation = self.rotation - self._worldcoords._translation = self.translation + self._worldcoords._rotation = self._rotation + self._worldcoords._translation = self._translation + finally: + if hook_disabled: + self._hook = original_hook self._changed = False def worldcoords(self): From 0fb43f296e14b3eca17a9716a1dafd9fba5590b7 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 3 Nov 2023 13:21:42 +0900 Subject: [PATCH 4/5] Add skip_normalization option for normalized axis --- skrobot/coordinates/base.py | 40 +++++++++++++++++++++++++------------ skrobot/coordinates/math.py | 20 +++++++++++++------ skrobot/model/joint.py | 3 ++- 3 files changed, 43 insertions(+), 20 deletions(-) diff --git a/skrobot/coordinates/base.py b/skrobot/coordinates/base.py index d29c0674..f26bd68a 100644 --- a/skrobot/coordinates/base.py +++ b/skrobot/coordinates/base.py @@ -70,8 +70,8 @@ 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._rotation = np.matmul(c1.rotation, c2.rotation) + out._translation = c1.translation + np.dot(c1._rotation, c2._translation) + out._rotation = np.matmul(c1._rotation, c2._rotation) return out @@ -958,7 +958,7 @@ def rotate_with_matrix(self, mat, wrt='local'): raise ValueError('wrt {} is not supported'.format(wrt)) return self - def rotate(self, theta, axis=None, wrt='local'): + def rotate(self, theta, axis=None, wrt='local', skip_normalization=False): """Rotate this coordinate by given theta and axis. This coordinate system is rotated relative to theta radians @@ -976,6 +976,8 @@ def rotate(self, theta, axis=None, wrt='local'): axis of rotation. The value of `axis` is represented as `wrt` frame. wrt : str or skrobot.coordinates.Coordinates + skip_normalization : bool + if `True`, skip normalization for axis. Returns ------- @@ -997,17 +999,23 @@ def rotate(self, theta, axis=None, wrt='local'): """ if isinstance(axis, list) or isinstance(axis, np.ndarray): self.rotate_with_matrix( - rotation_matrix(theta, axis), wrt) + rotation_matrix(theta, axis, + skip_normalization=skip_normalization), wrt) elif axis is None or axis is False: self.rotate_with_matrix(theta, wrt) elif wrt == 'local' or wrt == self: - self._rotation = rotate_matrix(self.rotation, theta, axis) + self._rotation = rotate_matrix( + self.rotation, theta, axis, + skip_normalization=skip_normalization) elif wrt == 'parent' or wrt == 'world': - self._rotation = rotate_matrix(self.rotation, theta, - axis, True) + self._rotation = rotate_matrix( + self.rotation, theta, + axis, True, + skip_normalization=skip_normalization) elif isinstance(wrt, Coordinates): # C1'=C2*R*C2(-1)*C1 self.rotate_with_matrix( - rotation_matrix(theta, axis), wrt) + rotation_matrix(theta, axis, + skip_normalization=skip_normalization), wrt) else: raise ValueError('wrt {} not supported'.format(wrt)) return self.newcoords(self.rotation, self.translation, @@ -1363,7 +1371,7 @@ def rotate_with_matrix(self, matrix, wrt): return self.newcoords(rotation, self.translation, check_validity=False) - def rotate(self, theta, axis, wrt='local'): + def rotate(self, theta, axis, wrt='local', skip_normalization=False): """Rotate this coordinate. Rotate this coordinate relative to axis by theta radians @@ -1376,6 +1384,8 @@ def rotate(self, theta, axis, wrt='local'): axis : str or numpy.ndarray 'x', 'y', 'z' or vector wrt : str or Coordinates + skip_normalization : bool + if `True`, skip normalization for axis. Returns ------- @@ -1383,21 +1393,25 @@ def rotate(self, theta, axis, wrt='local'): """ if isinstance(axis, list) or isinstance(axis, np.ndarray): return self.rotate_with_matrix( - rotation_matrix(theta, axis), wrt) + rotation_matrix(theta, axis, + skip_normalization=skip_normalization), wrt) if isinstance(axis, np.ndarray) and axis.shape == (3, 3): 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, 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, check_validity=False) else: return self.rotate_with_matrix( - rotation_matrix(theta, axis), wrt) + rotation_matrix(theta, axis, + skip_normalization=skip_normalization), wrt) def orient_with_matrix(self, rotation_matrix, wrt='world'): """Force update this coordinate system's rotation. diff --git a/skrobot/coordinates/math.py b/skrobot/coordinates/math.py index ab61432f..8f3b13da 100644 --- a/skrobot/coordinates/math.py +++ b/skrobot/coordinates/math.py @@ -388,7 +388,7 @@ def transform(m, v): return np.matmul(m, v) -def rotation_matrix(theta, axis): +def rotation_matrix(theta, axis, skip_normalization=False): """Return the rotation matrix. Return the rotation matrix associated with counterclockwise rotation @@ -401,6 +401,8 @@ def rotation_matrix(theta, axis): axis : str or list or numpy.ndarray rotation axis such that 'x', 'y', 'z' [0, 0, 1], [0, 1, 0], [1, 0, 0] + skip_normalization : bool + if `True`, skip normalization for axis. Returns ------- @@ -420,8 +422,9 @@ def rotation_matrix(theta, axis): [ 0.00000000e+00, 1.00000000e+00, 0.00000000e+00], [-1.00000000e+00, 0.00000000e+00, 2.22044605e-16]]) """ - axis = convert_to_axis_vector(axis) - axis = axis / np.sqrt(np.dot(axis, axis)) + if not skip_normalization: + axis = convert_to_axis_vector(axis) + axis = axis / np.sqrt(np.dot(axis, axis)) a = np.cos(theta / 2.0) b, c, d = -axis * np.sin(theta / 2.0) aa, bb, cc, dd = a * a, b * b, c * c, d * d @@ -466,10 +469,15 @@ def rotate_vector(vec, theta, axis): return rotated_vec -def rotate_matrix(matrix, theta, axis, world=None): +def rotate_matrix(matrix, theta, axis, world=None, skip_normalization=False): if world is False or world is None: - return np.dot(matrix, rotation_matrix(theta, axis)) - return np.dot(rotation_matrix(theta, axis), matrix) + return np.dot( + matrix, + rotation_matrix(theta, axis, + skip_normalization=skip_normalization)) + return np.dot( + rotation_matrix(theta, axis, skip_normalization=skip_normalization), + matrix) def rpy_matrix(az, ay, ax): diff --git a/skrobot/model/joint.py b/skrobot/model/joint.py index ff08bb6c..6863766a 100644 --- a/skrobot/model/joint.py +++ b/skrobot/model/joint.py @@ -242,7 +242,8 @@ def joint_angle(self, v=None, relative=None, enable_hook=True): v = self.min_angle diff_angle = v - self._joint_angle self._joint_angle = v - self.child_link.rotate(diff_angle, self.axis) + self.child_link.rotate(diff_angle, self.axis, + skip_normalization=True) if enable_hook: for hook in self._hooks: hook() From a52e79c736f4b4f358ba6c83d5ff7858497c5a63 Mon Sep 17 00:00:00 2001 From: iory Date: Fri, 3 Nov 2023 13:55:08 +0900 Subject: [PATCH 5/5] 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):