klampt.math.se3 module

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() and klampt.io.loader.read_se3().

klampt.math.se3.identity()[source]

Returns the identity transformation.

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.inv(T)[source]

Returns the inverse of the transformation.

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.apply(T, point)[source]

Applies the transform T to the given point

Return type

Sequence[float]

klampt.math.se3.apply_rotation(T, point)[source]

Applies only the rotation part of T

Return type

Sequence[float]

klampt.math.se3.rotation(T)[source]

Returns the 3x3 rotation matrix corresponding to T’s rotation

Return type

Sequence[float]

klampt.math.se3.from_rotation(mat)[source]

Returns a transformation T corresponding to the 3x3 rotation matrix mat

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.translation(T)[source]

Returns the translation vector corresponding to T’s translation

Return type

Sequence[float]

klampt.math.se3.from_translation(t)[source]

Returns a transformation T that translates points by t

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.homogeneous(T)[source]

Returns the 4x4 homogeneous transform corresponding to T.

Return type

List[List[float]]

klampt.math.se3.from_homogeneous(mat)[source]

Returns an se3 transform corresponding to the 4x4 homogeneous transform matrix mat.

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.ndarray(T)[source]

Returns the 4x4 homogeneous transform corresponding to T.

Return type

ndarray

klampt.math.se3.from_ndarray(mat)[source]

Returns an se3 transform from a 4x4 Numpy array representing the homogeneous transform.

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.mul(T1, T2)[source]

Composes two transformations.

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.distance(T1, T2, Rweight=1.0, tweight=1.0)[source]

Returns a distance metric between the two transformations. The rotation distance is weighted by Rweight and the translation distance is weighted by tweight

Return type

float

klampt.math.se3.error(T1, T2)[source]

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]), vectorops.add(v,T2[1])).

Return type

float

klampt.math.se3.interpolate(T1, T2, u)[source]

Interpolate linearly between the two transformations T1 and T2.

Return type

Tuple[Sequence[float], Sequence[float]]

klampt.math.se3.interpolator(T1, T2)[source]

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).

Return type

Callable