Created
September 7, 2016 02:52
-
-
Save limabeans/5aef174b1d6fda22898f96a3621114f4 to your computer and use it in GitHub Desktop.
euler rotations for robotics
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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,nsin(a)],[0,1,0],[sin(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 | |
def compose(third,second,first): | |
tmp = second.dot(first) | |
return third.dot(tmp) | |
def compose(second,first): | |
return second.dot(first) | |
# sample rotation | |
R = compose(Ry(-1*PI/2),Rx(PI/2)) | |
print mul(R,Y) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment