Klamp't Tutorial: Math

This tutorial will describe some basic math routines in Klamp't. Specifically, it will give a roadmap to help you find functions for operating with 3D math, rotation representations, and linear algebra.

Please note that these routines do not manage the semantics of points and coordinate frames! You will need to have a basic understanding of the mathematics of coordinate transformations and kinematics to use these functions effectively. For the more casual user, the experimental Python klampt.coordinates module in the version_0_6_2 branch of the Git repository will have convenience functions for managing coordinate transformations with less mental effort.

3D Points / Directions

The Klamp't C++ API represents points and directions using the Math3D::Vector3 class in KrisLibrary/math3d/primitives.h. This class has many convenience methods, including adding, subtracting, multiplying, and dividing using the standard operators +, -, *, and /. The individual elements of the vector are given by the x, y, and z members, or you may also use array access [0], [1], and [2].

```#include <math3d/primitives.h>
#include <iostream>
using namespace Math3D;

int main(int argc,char** argv) {
Vector3 a(0.3,1.5,-2.4);
printf("original: %g %g %g\n",a.x,a.y,a.z);
Vector3 b=a*4.0;   //scale by 4
printf("times 4: %g %g %g\n",b.x,b.y,b.z);
Vector3 c = b-a;   //subtraction
std::cout<<"b-a: "<<c<<std::endl;
c.inplaceNormalize();   //change to unit vector
std::cout<<"normalized: "<<c<<std::endl;
return 0;
}
```

Please consult the Vector3 documentation for more details.

3D Rotations

The Klamp't C++ API typically represents 3D rotations as a Math3D::Matrix3 object, as found in the KrisLibrary/math3d/primitives.h file.

For some basic operation, try the following code:

```#include <math3d/primitives.h>
#include <math/math.h>
#include <iostream>
using namespace Math3D;

int main(int argc,char** argv) {
Matrix3D A;
A.setIdentity();
std::cout<<"Identity: "<<A<<std::endl;
A.setRotateZ(DtoR(90.0));
std::cout<"90 degree rotation about Z: "<<A<<std::endl;

Vector3 x(1.0,2.0,3.0),y;
std::cout<<"Applied to: "<<x<<": "<<A*x<<endl;
A.mul(x,y);   //does the same as y=A*x, but does
//not construct a temporary object
std::cout<<"Applied to: "<<x<<": "<<y<<endl;
return 0;
}
```

Klamp't also supports conversions to four other commonly used rotation representations: Euler angles, axis-angle, moment (aka exponential map), and quaternions. These representations are given in the KrisLibrary/math3d/rotation.h file. Each representation can be converted to/from a Matrix3 representation using the getMatrix() and setMatrix() functions.

1. Euler angle representations are given by the EulerAngleRotation class, and consist of three angles phi, psi, theta about some axes A, B, and C. The axes are given by convention, some of the most common being roll-pitch-yaw (A.K.A. Z-Y-X) and Z-Y-Z convention. To convert from a matrix, the convention must be specified in the suffixes setMatrix[ABC]() and getMatrix[ABC]().
2. Axis-angle representations are given by the AxisAngleRotation class, and consist of a Vector3 axis and a Real angle (in radians).
3. Moment representations are given by the MomentRotation class, and are very similar to axis-angle representations but are more compact. They are a Vector3 (mx,my,mz) equivalent to axis*angle.
4. Quaternion representations are given by the QuaternionRotation class, and are a 4-tuple (x,y,z,w) representing a unit quaternion.

Rotation composition can be computed by the operator A*B or the Matrix3.mul(A,B) method. Note that the result corresponds to a rotation first by B, and then by A. (Recall that rotation composition is not symmetric! A*B != B*A unless the two rotations share the same axis of rotation) Inversion of a rotation is accomplished via the Matrix3.setTranspose(A) function (this assumes A is a rotation, because inversion is equivalent to the matrix transpose, since rotation matrices are orthogonal.) There is also a setInverse() method which will compute the same result, but setTranspose() is faster.

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 of the matrices. The KrisLibrary/math3d/interpolation.h header provides functionality for properly computing geodesics on SO(3). Specifically, interpolateRotation should be used. The absolute angle (calculated via the axis-angle representation) is also the most appropriate way to represent distances between rotations.

```#include <math3d/primitives.h>
#include <math3d/interpolate.h>
#include <math3d/rotation.h>
#include <math/math.h>
#include <iostream>
using namespace Math3D;

int main(int argc,char** argv) {
Matrix3D A,B;
A.setRotateZ(DtoR(90.0));
B.setRotateX(DtoR(-90.0));
#WRONG WAY! SO3 is not a cartesian space
#std::cout<<"Halfway: "<<(A+B)*0.5<<std::endl;
#std::cout<<"Difference: "<<(B-A)*0.5<<std::endl;

#RIGHT WAY!
AxisAngleRotation aa;
Matrix3 diff,temp;
diff.mulTransposeB(A,B);
aa.setMatrix(diff);
std::cout<<"Distance: "<<Abs(aa.angle)<<std::endl;
interpolateRotation(A,B,0.0,temp);
std::cout<<"Start of interpolation: "<<temp<<std::endl;
interpolateRotation(A,B,0.5,temp);
std::cout<<"Halfway: "<<temp<<std::endl;
interpolateRotation(A,B,1.0,temp);
std::cout<<"End of interpolation: "<<temp<<std::endl;
return 0;
}
```
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 via the Math3D::RigidTransform class. The object simply consists of members Matrix3 R and Vector3 t.

You may apply a transform T to a Vector3 x using T*x, or the function T.mul(x,y), or T.mulPoint(x,y). 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 T.R*x or via the convenience function T.mulVector(x,y)

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

Linear Algebra

Klamp't comes with a large suite of linear algebra routines in the KrisLibrary library. The Vector class in KrisLibrary/math/vector.h contains basic linear algebra routines on vectors (adding, subtracting, multiplying, interpolating). The Matrix class in KrisLibrary/math/matrix.h contains basic matrix-matrix and matrix-vector operations. There are also various sparse vector / sparse matrix structures available. Note that these are designed primarily for convenience, and are unlikely to be quite as fast as more specialized packages for basic operations (e.g., BLAS, ATLAS, Intel MKL).

Linear algebra routines are available for the following operations:

1. Gram-Schmidt orthogonalization
2. LU decomposition
3. Cholesky/LDL decomposition
4. QR decomposition
5. Singular value decomposition

Note that these are designed primarily for convenience and robustness rather than speed, and you may get better performance using other packages. Please consult the KrisLibrary documentation for more details.

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()
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
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); vectorops.dot(a,b) numpy.dot(a,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 [vectorops.dot(ai,b) for ai in A] numpy.dot(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
```

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

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

#RIGHT WAY!
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 vectorops 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 euler-angle representation eul2rotm([theta phi psi],'ZYX') so3.mul(so3.rotation([0,0,1],theta), so3.mul(so3.rotation([0,1,0],phi), so3.rotation([1,0,0],psi))) 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.from_quaternion([w,x,y,z]) numpy.dot(R,x)
Compose rotation R1 followed by R2 R2*R1 so3.mul(R2,R1) numpy.dot(R2,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
#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 vectorops 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.dot(T,numpy.append(x,[1]))[0:3]
Compose transform T1 followed by T2 T2*T1 se3.mul(T2,T1) numpy.dot(T2,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

`x.tolist()`

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

`numpy.array(x)`

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.