euler rotations for robotics

euler.py

import numpy as np
# NOTE: numpy uses radians for its angles

# constants
PI = np.pi
cos = lambda ang: np.cos(ang)
ncos = lambda ang: -1*np.cos(ang)
sin = lambda ang: np.sin(ang)
nsin = lambda ang: -1*np.sin(ang)

# common numbers
# cos(PI/6) = sin(PI/3) = 0.866
# cos(PI/3) = cos(PI/3) = 0.5
# cos(PI/4) = sin(PI/4) = 0.707

# Rotation matrices
Rz = lambda a: np.array([[cos(a),nsin(a),0],[sin(a),cos(a),0],[0,0,1]])
Ry = lambda a: np.array([[cos(a),0,sin(a)],[0,1,0],[nsin(a),0,cos(a)]])
Rx = lambda a: np.array([[1,0,0],[0,cos(a),nsin(a)],[0,sin(a),cos(a)]])

# create 3D vectors easily
vec3 = lambda v:  np.array([ [v[0]], [v[1]], [v[2]] ])
X = vec3([1,0,0])
Y = vec3([0,1,0])
Z = vec3([0,0,1])

mul = lambda R, v: R.dot(v)


# return a new matrix that applies first, second, third
# or could just do two rotations as well if wanted
def compose(third,second,first=None):
    if first == None:
        return third.dot(second)
    return third.dot(second.dot(first))


# sample rotation
R = compose(Ry(-1*PI/2),Rx(PI/2))
print mul(R,Y)