# Source code for klampt.math.se3

```"""Operations for rigid transformations in Klamp't.  All rigid
transformations are specified in the form ``T=(R,t)``,
where R is a 9-list specifying the entries of the rotation matrix in column
major form, and t is a 3-list specifying the translation vector.

Primarily, this form was chosen for interfacing with C++ code.  C++ methods
on objects of the form ``T = X.getTransform()`` will return se3 element proper.
Conversely, ``X.setTransform(*T)`` will set the rotation and translation of
some transform.

Extracting the so3 portion of the transform is simply T[0].  Extracting
the translation vector is simply T[1].

Applying an se3 element to a point p is ``apply(T,p)``.  Applying it to a
directional quantity ``d`` is either ``apply_rotation(T,d)`` or
``so3.apply(T[0],d)``.

To flatten into a single array, use ``klampt.model.getConfig(T)``, which is
equivalent to ``T[0] + T[1]``.  To read and write to disk in a way that is
compatible with  other Klamp't IO routines, use ``klampt.io.loader.write_se3()``
"""

from . import vectorops
from . import so3
from typing import List,Tuple,Callable
from ..model.typing import Vector3,Rotation,RigidTransform

[docs]def identity() -> RigidTransform:
"""Returns the identity transformation."""
return ([1.,0.,0.,0.,1.,0.,0.,0.,1.],[0.,0.,0.])

[docs]def inv(T : RigidTransform) -> RigidTransform:
"""Returns the inverse of the transformation."""
(R,t) = T
Rinv = so3.inv(R)
tinv = [-Rinv[0]*t[0]-Rinv[3]*t[1]-Rinv[6]*t[2],
-Rinv[1]*t[0]-Rinv[4]*t[1]-Rinv[7]*t[2],
-Rinv[2]*t[0]-Rinv[5]*t[1]-Rinv[8]*t[2]]
return (Rinv,tinv)

[docs]def apply(T : RigidTransform, point: Vector3) -> Vector3:
"""Applies the transform T to the given point"""

[docs]def apply_rotation(T : RigidTransform, point : Vector3) -> Vector3:
"""Applies only the rotation part of T"""
R = T[0]
return so3.apply(R,point)

[docs]def rotation(T : RigidTransform) -> Rotation:
"""Returns the 3x3 rotation matrix corresponding to T's rotation"""
(R,t) = T
return so3.matrix(R)

[docs]def from_rotation(mat : List[List[float]]) -> RigidTransform:
"""Returns a transformation T corresponding to the 3x3 rotation matrix mat"""
R = so3.from_matrix(mat)
return (R,[0.,0.,0.])

[docs]def translation(T : RigidTransform) -> Vector3:
"""Returns the translation vector corresponding to T's translation"""
return T[1]

[docs]def from_translation(t : Vector3) -> RigidTransform:
"""Returns a transformation T that translates points by t"""
return (so3.identity(),t[:])

[docs]def homogeneous(T : RigidTransform) -> List[List[float]]:
"""Returns the 4x4 homogeneous transform corresponding to T."""
(R,t) = T
return [[R[0],R[3],R[6],t[0]],
[R[1],R[4],R[7],t[1]],
[R[2],R[5],R[8],t[2]],
[0.,0.,0.,1.]]

[docs]def from_homogeneous(mat : List[List[float]]) -> RigidTransform:
"""Returns an se3 transform corresponding to the 4x4 homogeneous
transform matrix mat."""
t = [mat[0][3],mat[1][3],mat[2][3]]
R = [mat[0][0],mat[1][0],mat[2][0],mat[0][1],mat[1][1],mat[2][1],mat[0][2],mat[1][2],mat[2][2]]
return (R,t)

[docs]def ndarray(T : RigidTransform) -> "ndarray":
"""Returns the 4x4 homogeneous transform corresponding to T."""
import numpy
return numpy.array(homogeneous(T))

[docs]def from_ndarray(mat : "ndarray") -> "RigidTransform":
"""Returns an se3 transform from a 4x4 Numpy array representing the
homogeneous transform."""
return from_homogeneous(mat)

[docs]def mul(T1 : RigidTransform, T2 : RigidTransform) -> RigidTransform:
"""Composes two transformations."""
if len(T1) != 2: raise ValueError("T1 is not a transform")
if len(T2) != 2: raise ValueError("T2 is not a transform (did you mean to use apply())?")
(R1,t1) = T1
(R2,t2) = T2
R = so3.mul(R1,R2)
return (R,t)

[docs]def distance(T1 : RigidTransform, T2 : RigidTransform, Rweight=1.0,tweight=1.0) -> float:
"""Returns a distance metric between the two transformations. The
rotation distance is weighted by Rweight and the translation distance
is weighted by tweight"""
(R1,t1)=T1
(R2,t2)=T2
return Rweight*so3.distance(R1,R2) + tweight*vectorops.distance(t1,t2)

[docs]def error(T1 : RigidTransform, T2 : RigidTransform) -> float:
"""Returns a 6D "difference vector" that describes how far T1 is from T2.
More precisely, this is the (stacked) Lie derivative (w,v).

Fun fact: the error is related to the derivative of interpolate(T2,T1,u)
at u=0 by d/du interpolate(T2,T1,0) = (mul(cross_product(w),R2),v).

You can also recover T1 from (w,v) via T1 = (mul(from_moment(w),T2[0]),
"""
(R1,t1)=T1
(R2,t2)=T2
#concatenate lists
return so3.error(R1,R2) + vectorops.sub(t1,t2)

[docs]def interpolate(T1 : RigidTransform, T2 : RigidTransform, u : float) -> RigidTransform:
"""Interpolate linearly between the two transformations T1 and T2."""
return (so3.interpolate(T1[0],T2[0],u),vectorops.interpolate(T1[1],T2[1],u))

[docs]def interpolator(T1 : RigidTransform, T2 : RigidTransform) -> Callable:
"""Returns a function of one parameter u that interpolates linearly
between the two transformations T1 and T2. After f(u) is constructed, calling
f(u) is about 2x faster than calling interpolate(T1,T2,u)."""
R1,t1 = T1
R2,t2 = T2
dt = vectorops.sub(t2,t1)
def f(u,so3_interp=so3.interpolator(R1,R2),t1=t1,dt=dt):