klampt.math.so3 module

Operations for rigid rotations in Klampt. All rotations are represented by a 9-list specifying the entries of the rotation matrix in column major form.

In other words, given a 3x3 matrix:

[a11,a12,a13]
[a21,a22,a23]
[a31,a32,a33],

Klamp’t represents the matrix as a list [a11,a21,a31,a12,a22,a32,a13,a23,a33].

The reasons for this representation are 1) simplicity, and 2) a more convenient interface with C code.

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

Returns the identity rotation.

Return type

Sequence[float]

klampt.math.so3.inv(R)[source]

Inverts the rotation.

Return type

Sequence[float]

klampt.math.so3.apply(R, point)[source]

Applies the rotation to a point.

Return type

Sequence[float]

klampt.math.so3.matrix(R)[source]

Returns the 3x3 rotation matrix corresponding to R.

Return type

Sequence[Sequence[float]]

klampt.math.so3.from_matrix(mat)[source]

Returns a rotation R corresponding to the 3x3 rotation matrix mat.

Return type

Sequence[float]

klampt.math.so3.ndarray(R)[source]

Returns the 3x3 numpy rotation matrix corresponding to R.

Return type

ndarray

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

Returns a rotation R corresponding to the 3x3 rotation matrix mat.

Return type

Sequence[float]

klampt.math.so3.mul(R1, R2)[source]

Multiplies two rotations.

Return type

Sequence[float]

klampt.math.so3.trace(R)[source]

Computes the trace of the rotation matrix.

Return type

float

klampt.math.so3.angle(R)[source]

Returns absolute deviation of R from identity

Return type

float

klampt.math.so3.rpy(R)[source]

Converts a rotation matrix to a roll,pitch,yaw angle triple. The result is given in radians.

Return type

Sequence[float]

klampt.math.so3.from_rpy(rollpitchyaw)[source]

Converts from roll,pitch,yaw angle triple to a rotation matrix. The triple is given in radians. The x axis is “roll”, y is “pitch”, and z is “yaw”.

Return type

Sequence[float]

klampt.math.so3.rotation_vector(R)[source]

Returns the rotation vector w (exponential map) representation of R such that e^[w] = R. Equivalent to axis-angle representation with w/||w||=axis, ||w||=angle.

Return type

Sequence[float]

klampt.math.so3.axis_angle(R)[source]

Returns the (axis,angle) pair representing R

Return type

Tuple

klampt.math.so3.from_axis_angle(aa)[source]

Converts an axis-angle representation (axis,angle) to a 3D rotation matrix.

Return type

Sequence[float]

klampt.math.so3.from_rotation_vector(w)[source]

Converts a rotation vector representation w to a 3D rotation matrix.

Return type

Sequence[float]

klampt.math.so3.moment(R)

Returns the rotation vector w (exponential map) representation of R such that e^[w] = R. Equivalent to axis-angle representation with w/||w||=axis, ||w||=angle.

Return type

Sequence[float]

klampt.math.so3.from_moment(w)

Converts a rotation vector representation w to a 3D rotation matrix.

Return type

Sequence[float]

klampt.math.so3.from_quaternion(q)[source]

Given a unit quaternion (w,x,y,z), produce the corresponding rotation matrix.

Return type

Sequence[float]

klampt.math.so3.quaternion(R)[source]

Given a Klamp’t rotation representation, produces the corresponding unit quaternion (w,x,y,z).

Return type

Tuple

klampt.math.so3.distance(R1, R2)[source]

Returns the absolute angle one would need to rotate in order to get from R1 to R2

Return type

float

klampt.math.so3.error(R1, R2)[source]

Returns a 3D “difference vector” that describes how far R1 is from R2. More precisely, this is the (local) Lie derivative, which is the rotation vector representation of R1*R2^T.

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

You can also recover R1 from w via R1 = mul(from_moment(w),R2).

Return type

float

klampt.math.so3.cross_product(w)[source]

Returns the cross product matrix associated with w.

The matrix [w]R is the derivative of the matrix R as it rotates about the axis w/||w|| with angular velocity ||w||.

Return type

Sequence[float]

klampt.math.so3.diag(R)[source]

Returns the diagonal of the 3x3 matrix reprsenting the so3 element R.

Return type

Sequence[float]

klampt.math.so3.deskew(R)[source]

If R is a (flattened) cross-product matrix of the 3-vector w, this will return w. Otherwise, it will return a representation w of (R-R^T)/2 (off diagonals of R) such that (R-R^T)/2 = cross_product(w).

Return type

Sequence[float]

klampt.math.so3.rotation(axis, angle)[source]

Given a unit axis and an angle in radians, returns the rotation matrix.

Return type

Sequence[float]

klampt.math.so3.canonical(v)[source]

Given a unit vector v, finds R that defines a basis [x,y,z] such that x = v and y and z are orthogonal

Return type

Sequence[float]

klampt.math.so3.align(a, b)[source]

Returns a minimal-angle rotation that aligns the vector a to align with the vector b. Both a and b must be nonzero.

Return type

Sequence[float]

klampt.math.so3.interpolate(R1, R2, u)[source]

Interpolate linearly between the two rotations R1 and R2.

Return type

Sequence[float]

klampt.math.so3.interpolator(R1, R2)[source]

Returns a function of one parameter u that interpolates linearly between the two rotations R1 and R2. After f(u) is constructed, calling f(u) is about 2x faster than calling interpolate(R1,R2,u).

Return type

Callable

klampt.math.so3.det(R)[source]

Returns the determinant of the 3x3 matrix R

Return type

float

klampt.math.so3.is_rotation(R, tol=1e-05)[source]

Returns true if R is a rotation matrix, i.e. is orthogonal to the given tolerance and has + determinant

Return type

bool

klampt.math.so3.sample()[source]

Returns a uniformly distributed rotation matrix.

Return type

Sequence[float]