# Source code for pytransform3d.transformations._transform_operations

"""Transform operations."""
import numpy as np
from ..rotations import (
axis_angle_from_matrix, matrix_from_axis_angle, norm_vector)
from ._utils import check_transform

[docs]def invert_transform(A2B, strict_check=True, check=True): """Invert transform. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrix is valid Returns ------- B2A : array-like, shape (4, 4) Transform from frame B to frame A """ if check: A2B = check_transform(A2B, strict_check=strict_check) # NOTE there is a faster version, but it is not faster than matrix # inversion with numpy: # ( R t )^-1 ( R^T -R^T*t ) # ( 0 1 ) = ( 0 1 ) return np.linalg.inv(A2B)
[docs]def vector_to_point(v): """Convert 3D vector to position. A point (x, y, z) given by the components of a vector will be represented by [x, y, z, 1] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- v : array-like, shape (3,) 3D vector that contains x, y, and z Returns ------- p : array-like, shape (4,) Point vector with 1 as last element """ return np.hstack((v, 1))
[docs]def vectors_to_points(V): """Convert 3D vectors to positions. A point (x, y, z) given by the components of a vector will be represented by [x, y, z, 1] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- V : array-like, shape (n_points, 3) Each row is a 3D vector that contains x, y, and z Returns ------- P : array-like, shape (n_points, 4) Each row is a point vector with 1 as last element """ return np.hstack((V, np.ones((len(V), 1))))
[docs]def vector_to_direction(v): """Convert 3D vector to direction. A direction (x, y, z) given by the components of a vector will be represented by [x, y, z, 0] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- v : array-like, shape (3,) 3D vector that contains x, y, and z Returns ------- p : array-like, shape (4,) Direction vector with 0 as last element """ return np.hstack((v, 0))
[docs]def vectors_to_directions(V): """Convert 3D vectors to directions. A direction (x, y, z) given by the components of a vector will be represented by [x, y, z, 0] in homogeneous coordinates to which we can apply a transformation. Parameters ---------- V : array-like, shape (n_directions, 3) Each row is a 3D vector that contains x, y, and z Returns ------- P : array-like, shape (n_directions, 4) Each row is a direction vector with 0 as last element """ return np.hstack((V, np.zeros((len(V), 1))))
[docs]def concat(A2B, B2C, strict_check=True, check=True): """Concatenate transformations. We use the extrinsic convention, which means that B2C is left-multiplied to A2B. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B B2C : array-like, shape (4, 4) Transform from frame B to frame C strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. check : bool, optional (default: True) Check if transformation matrices are valid Returns ------- A2C : array-like, shape (4, 4) Transform from frame A to frame C """ if check: A2B = check_transform(A2B, strict_check=strict_check) B2C = check_transform(B2C, strict_check=strict_check) return B2C.dot(A2B)
[docs]def transform(A2B, PA, strict_check=True): """Transform point or list of points or directions. Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B PA : array-like, shape (4,) or (n_points, 4) Point or points in frame A strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- PB : array-like, shape (4,) or (n_points, 4) Point or points in frame B Raises ------ ValueError If dimensions are incorrect """ A2B = check_transform(A2B, strict_check=strict_check) PA = np.asarray(PA) if PA.ndim == 1: return np.dot(A2B, PA) if PA.ndim == 2: return np.dot(PA, A2B.T) raise ValueError("Cannot transform array with more than 2 dimensions")
[docs]def scale_transform(A2B, s_xr=1.0, s_yr=1.0, s_zr=1.0, s_r=1.0, s_xt=1.0, s_yt=1.0, s_zt=1.0, s_t=1.0, s_d=1.0, strict_check=True): """Scale a transform from A to reference frame B. See algorithm 10 from "Analytic Approaches for Design and Operation of Haptic Human-Machine Interfaces" (Bertold Bongardt). Parameters ---------- A2B : array-like, shape (4, 4) Transform from frame A to frame B s_xr : float, optional (default: 1) Scaling of x-component of the rotation axis s_yr : float, optional (default: 1) Scaling of y-component of the rotation axis s_zr : float, optional (default: 1) Scaling of z-component of the rotation axis s_r : float, optional (default: 1) Scaling of the rotation s_xt : float, optional (default: 1) Scaling of z-component of the translation s_yt : float, optional (default: 1) Scaling of z-component of the translation s_zt : float, optional (default: 1) Scaling of z-component of the translation s_t : float, optional (default: 1) Scaling of the translation s_d : float, optional (default: 1) Scaling of the whole transform (displacement) strict_check : bool, optional (default: True) Raise a ValueError if the transformation matrix is not numerically close enough to a real transformation matrix. Otherwise we print a warning. Returns ------- A2B_scaled Scaled transform from frame A to frame B (actually this is a transform from A to another frame C) """ A2B = check_transform(A2B, strict_check=strict_check) A2B_scaled = np.eye(4) R = A2B[:3, :3] t = A2B[:3, 3] S_t = np.array([s_xt, s_yt, s_zt]) A2B_scaled[:3, 3] = s_d * s_t * S_t * t a = axis_angle_from_matrix(R) a_new = np.empty(4) a_new[3] = s_d * s_r * a[3] S_r = np.array([s_xr, s_yr, s_zr]) a_new[:3] = norm_vector(S_r * a[:3]) A2B_scaled[:3, :3] = matrix_from_axis_angle(a_new) return A2B_scaled