Math concepts

Klamp’t assumes basic familiarity with 3D geometry and linear algebra concepts. It heavily uses structures that representing vectors, matrices, 3D points, 3D rotations, and 3D transformations. These routines are heavily tested and fast.

The main mathematical objects used in Klampt are as follows:

  • Vector: a variable-length (n-D) vector.
  • Matrix: a variable-size (m x n) matrix.
  • Vector3: a 3-D vector.
  • Matrix3: a 3x3 matrix.
  • Rotation: a 3D rotation, specifically an element of the special orthogonal group SO(3), usually represented by a Matrix3.
  • RigidTransform: a rigid transformation T(x) = R*x + t, with R a rotation and t a Vector3

Illustration of concepts

API summary

3D math operations are found in the klampt.math module under the following files.

  • vectorops: basic vector operations on lists of numbers.
  • so2: routines for handling 2D rotations.
  • so3: routines for handling 3D rotations.
  • se3: routines for handling 3D rigid transformations

The use of numpy / scipy is recommended if you are doing any significant linear algebra.

3D Points / Directions

The Klamp’t Python API represents points and directions simply as 3-lists or 3-tuples of floats. To perform operations on such objects, the klampt.math.vectorops module has functions for adding, subtracting, multiplying, normalizing, and interpolating. To summarize, the following table lists major vector operations in Matlab, the Klamp’t vectorops module, and Numpy/Scipy.

Operation Matlab Klamp’t vectorops Numpy/Scipy
Import N/A from klampt.math import vectorops import numpy
Create vector from list of elements a=[1 2 3]; a=[1,2,3] a=numpy.array([1,2,3])
Copy vector b=a; b=a[:] b=a.copy() or b=numpy.array(a)
Create vector of zeros a=zeros(3,1); a=[0]*3 a=numpy.zeros(3) or a=numpy.array([0]*3)
Access first element a(1) a[0] a[0]
Concatenate two vectors [a b] a+b concatenate((a,b))
Getting elements as a Python list N/A a a.tolist()
Add vectors a+b vectorops.add(a,b) a+b
Subtract vectors a-b vectorops.sub(a,b) a-b
Multiply vector by scalar a*b vectorops.mul(a,b) a*b
Divide vector by scalar a/b vectorops.div(a,b) a/b
Add vector to vector times scalar (multiply-add) a+b*c vectorops.madd(a,b,c) a+b*c
Incremental add vectors a=a+b; a=vectorops.add(a,b) a+=b
Incremental subtract vectors a=a-b; a=vectorops.sub(a,b) a-=b
Incremental multiply vector by scalar a=a*b; a=vectorops.mul(a,b) a*=b
Incremental divide vector by scalar a=a/b; a=vectorops.div(a,b) a/=b
Elementwise multiply vectors a.*b; vectorops.mul(a,b) a*b or numpy.multiply(a,b)
Dot product dot(a,b);,b),b)
L2 norm norm(a); vectorops.norm(a) numpy.linalg.norm(a)
Squared L2 norm norm(a)^2 or dot(a,a) vectorops.normSquared(a) (faster than norm) numpy.linalg.norm(a)**2
L2 distance norm(a-b) vectorops.distance(a,b) numpy.linalg.norm(a-b)
Squared L2 distance norm(a-b)^2 or dot(a-b,a-b) vectorops.distanceSquared(a) (faster than distance) numpy.linalg.norm(a-b)**2
Cross product cross(a,b) vectorops.cross(a,b) numpy.cross(a,b)
Interpolate between vectors a and b with parameter u a+u*(b-a) vectorops.interpolate(a,b,u) a+u*(b-a)
Matrix-vector multiply A*b [,b) for ai in A],b)

3D Rotations

The Klamp’t Python API represents a 3D rotation as a 9-tuple (a11,a21,a31,a21,a22,a32,a31,a32,a33) listing each of the entries of the rotation matrix in column-major order. The klampt.math.so3 module provides several operations on rotations in this representation, as well as conversions to and from alternate representations. (The so3 module is so named because the space of rotations is known as the special orthogonal group SO(3)).

For some basic operation, try the following code:

from klampt.math import so3,vectorops

    A = so3.identity()  #builds an identity rotation
    print "Original:",A  #prints [1,0,0,0,1,0,0,0,1]
    #pretty-print the rotation matrix
    print "Pretty printed:",so3.__str__(A)
    #returns the 2D array form of A
    print "matrix()",so3.matrix(A)

    point = [3.0,1.5,-0.4]  #make some point
    #Apply the rotation A to the point.
    print so3.apply(A,point)
    #Since it's an identity, the point does not change

Try it again with a 90 degree rotation about the z axis, by replacing the assignment to A with the following: A=[0,1,0,-1,0,0,0,0,1]. Observe that the printed point is now rotated by 90 degrees from the original point.

We can also produce rotation matrices using the so3.rotation(axis,angle) function. The axis is a unit vector (given by a 3-tuple) and the angle is given in radians. So, to construct the 90 degree rotation about Z we used above, we can avoid fussing about the ordering of elements in the 9-tuple, by simply using the following code:

import math
from klampt.math import so3

#first argument is the axis, second argument is the angle in radians
print so3.rotation((0,0,1),math.radians(90))

Klamp’t also supports conversions to three other commonly used rotation representations: axis-angle, moment (aka exponential map), and quaternions.

  1. Axis-angle representations we have seen above, and are simply a pair (axis,angle). To convert to/from an so3 element, use so3.from_axis_angle() and so3.axis_angle()
  2. Moment representations are very similar to axis-angle representations but are more compact. They are a 3-tuple (mx,my,mz) equivalent to axis*angle. To convert to/from an so3 element use so3.from_moment() and so3.moment().
  3. Quaternion representations are 4-tuples representing a unit quaternion. To convert to/from an so3 element use so3.from_quaternion() and so3.quaternion().

Rotations can also be composed using the so3.mul(A,B) function. Note that the result corresponds to a rotation first by B, and then by A. (Recall that rotation composition is not symmetric! so3.mul(A,B) != so3.mul(B,A) unless the two rotations share the same axis of rotation) Inversion of a rotation is accomplished via the so3.inv(A) function. Inversion is equivalent to the matrix transpose, since rotation matrices are orthogonal.

The space of rotations is fundamentally different from Cartesian space, and hence computing interpolations and finding the difference between rotations is not as simple as taking standard linear interpolations in the 9-D space. The klampt.so3 module provides functionality for properly computing geodesics on SO(3).

from klampt.math import vectorops,so3
import math

A = so3.rotation((0,0,1),math.radians(90))
B = so3.rotation((1,0,0),math.radians(-90))

#WRONG WAY! SO3 is not a cartesian space
#print "Distance:",vectorops.distance(A,B)
#print "Halfway:",vectorops.interpolate(A,B,0.5)
#print "Difference:",vectorops.sub(B,A)

print "Distance:",so3.distance(A,B)
print "Start of interpolation:",so3.interpolate(A,B,0)
print "Halfway:",so3.interpolate(A,B,0.5)
print "End of interpolation:",so3.interpolate(A,B,1)
print "Lie derivative:",so3.error(A,B)

The last term is a 3-tuple indicating the amounts by which A would need to be rotated about its local x, y and z axes to get to B.

The following table summarizes the major SO(3) operations in Matlab, the Klamp’t so3 module, and Numpy/Scipy.

Operation Matlab (Robotics toolbox) Klamp’t so3 Numpy/Scipy
Create SO(3) identity eye(3) so3.identity() numpy.eye(3)
Create SO(3) from 3x3 matrix a so3.from_matrix(a) a
Create from axis-angle representation axang2rotm([x y z rads]) so3.rotation([x,y,z],rads) N/A
Create from rotation vector representation rads=norm(w); axang2rotm([w/rads rads]) so3.from_rotation_vector(w) N/A
Create from euler-angle representation eul2rotm([theta phi psi],’ZYX’) so3.from_rpy((psi,phi,theta)) N/A
Create from quaternion representation quat2rotm([w x y z]) so3.from_quaternion([w,x,y,z]) N/A
Apply rotation to point R*x so3.apply(R,x),x)
Compose rotation R1 followed by R2 R2*R1 so3.mul(R2,R1),R1)
Invert rotation R’ so3.inv(R) R.T or numpy.transpose(R)
Interpolate rotations R1 and R2 N/A so3.interpolate(R1,R2,u) N/A
Angular difference between R1 and R2 abs(rotm2axang(R1’*R2)[4]) so3.angle(R1,R2) N/A

Rigid Transformations

Rigid transformations are used throughout Klamp’t, and represent an function y = R*x+t, where R is a 3x3 rotation matrix, t is a 3D translation vector, x is the input 3D point, and y is the 3D output point. The transform is represented throughout the Klamp’t Python API as a pair (R,t), and operations on transforms are given by the klampt.math.se3 module. (It is so named because the mathematic space of transformations is known as the special euclidean group SE(3)).

To construct a transform, you will typically create the elements R and t with whatever methods you wish, then assemble the pair (R,t). To extract R or t, you will use the tuple indices [0] or [1], respectively. If you are doing many operations on the components of a transform A, it may also be convenient to use the unpacking semantics (R,t) = A.

from klampt.math import vectorops,so3,se3
import math

#make an identity rigid transform
A = se3.identity()
#make a 90 degree rotation about the z axis plus a 3-unit
#shift in the x axis
B = (so3.rotation((0,0,1),math.radians(90)),[3.0,0,0])
#make a transform using A's rotation and B's translation
C = (A[0],B[1])
#make a transform that has the inverse of B's rotation,
#with 4x the translation.
#First unpack the rotation and translation of the transform B
R,t = B
#Then make it
D = (so3.inv(R),vectorops.mul(t,4.0))

You may apply a transform to a point x using the function se3.apply(T,x). If x is a direction vector, and you wish to apply only the rotation part of the transform, you can either do this manually via so3.apply(T[0],x) or via the convenience function so3.apply_rotation(T,x)

Transforms may be composed using the se3.mul(A,B) function and inverted using the se3.inv(A) function.

Interpolation, distance, and errors (Lie derivatives) are similar to the so3 module. The se3.distance(A,B,Rweight=1,tweight=1) function also takes optional weighting parameters that describe how the rotation and translation components should be weighted when computing distance.

The following table summarizes the major SE(3) operations in Matlab, the Klamp’t so3 module, and Numpy/Scipy.

Operation Matlab (Robotics toolbox) Klamp’t se3 Numpy/Scipy
Create SE(3) identity eye(4) se3.identity() numpy.eye(4)
Create SE(3) from 4x4 homogeneous matrix a se3.from_homogeneous(a) a
Create from SO(3) element and translation vector trvec2tform(t)*rotm2tform(R) (R,t) T=numpy.eye(4); T[:3,:3]=R; T[:3,3]=t;
Extract rotation (SO(3) element) tform2rotm(T) T[0] T[0:3,0:3]
Extract translation vector tform2trvec(T) T[1] T[0:3,3].flatten()
Apply transform to point (T*[x 1])(1:3) se3.apply(T,x),numpy.append(x,[1]))[0:3]
Compose transform T1 followed by T2 T2*T1 se3.mul(T2,T1),T1)
Invert transform T’ se3.inv(T) T.T or numpy.transpose(T)
Interpolate transforms T1 and T2 N/A se3.interpolate(T1,T2,u) N/A
Distance between T1 and T2 N/A se3.distance(T1,T2) N/A

Linear Algebra

For basic linear algebra on vectors (adding, subtracting, multiplying, interpolating), the klampt.vectorops module contains a suite of functions. It is very lightweight and works nicely with vectors represented as native Python lists.

We recommend using Numpy/Scipy for more sophisticated linear algebra functionality, such as matrix operations. Note that Klamp’t routines return/accept raw lists of numbers, not Numpy arrays. Hence, you must use the


method to convert a Numpy array x for use with Klamp’t routines, or


to convert a list to a Numpy array object.

To work with rotation matrices in Numpy/Scipy, use the so3.matrix()/so3.from_matrix() routines to convert to and from 2-D arrays, respectively. Similarly, to work with rigid transformation matrices, use se3.homogeneous()/se3.from_homogeneous() to get a representation of the transform as a 4x4 matrix in homogeneous coordinates.