Moved the matrix util python into the sot core package.

python/matrix_util.py

-
-'''
-Tiny matrix functions, taken from Oscar source code.
-'''
-
-#--------------------------------------------------------------------------
-#--------------------------------------------------------------------------
-#--------------------------------------------------------------------------
-from math import *
-from numpy import *
-from random import random
-
-# Convert matrix to tuple
-def matrixToTuple(M):
-    tmp = M.tolist()
-    res = []
-    for i in tmp:
-        res.append(tuple(i))
-    return tuple(res)
-
-def vectorToTuple(M):
-    if len(M.shape)==1:      return tuple(M.tolist())
-    elif M.shape[0]==1:      return tuple(M.tolist()[0])
-    else:                    return tuple(M.transpose().tolist()[0])
-
-# Convert from Roll, Pitch, Yaw to transformation Matrix
-def rpy2tr(r,p,y):
-    mat = matrix(rotate('z',y))*matrix(rotate('y',p))*matrix(rotate('x',r))
-    return matrixToTuple(mat)
-
-# Get the distance btw the position components of 2 transf matrices
-def distVector(M2,M1):
-    px = M1[0][3] - M2[0][3]
-    py = M1[1][3] - M2[1][3]
-    pz = M1[2][3] - M2[2][3]
-    return [px,py,pz]
-
-# Obtain an orthonormal matrix SO3 using the given vector as its first column
-# (it computes Gram Schmidt to obtain an orthonormal basis using the
-#  first vector and 2 other 'random' vectors)
-
-def generateOrthonormalM(v1):
-
-    v2 = matrix([random(),random(),random()])
-    v3 = matrix([random(),random(),random()])
-
-    v1 = matrix(v1)
-    e1 = v1/linalg.norm(v1)
-
-    u2 = v2-inner(v2,e1)*e1
-    e2 = u2/linalg.norm(u2)
-
-    u3 = v3-inner(v3,e1)*e1-inner(v3,e2)*e2
-    e3 = u3/linalg.norm(u3)
-
-    e1=e1.tolist(); e2=e2.tolist(); e3=e3.tolist()
-    M = ( (e1[0][0],e2[0][0],e3[0][0]), (e1[0][1],e2[0][1],e3[0][1]), (e1[0][2],e2[0][2],e3[0][2]) )
-    return M
-
-# Convert from Transformation Matrix to Roll,Pitch,Yaw
-def tr2rpy(M):
-    m = sqrt(M[2][1]**2+M[2][2]**2)
-    p = atan2(-M[2][0],m)
-
-    if abs( p-pi/2 ) < 0.001:
-        r = 0
-        y = atan2(M[0][1],M[1][1])
-    elif abs( p+pi/2 ) < 0.001:
-        r = 0
-        y = -atan2(M[0][1],M[1][1])
-    else:
-        r = atan2(M[1][0],M[0][0])
-        y = atan2(M[2][1],M[2][2])
-
-    return [float(r),float(p),float(y)]
-
-def matrixToRPY( M ):
-    '''
-    Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
-    '''
-    rot = tr2rpy(M)
-    return [ M[0][3], M[1][3], M[2][3], rot[2],rot[1],rot[0]]
-
-def RPYToMatrix( pr ):
-    '''
-    Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
-    '''
-    M=array(rpy2tr(pr[3],pr[4],pr[5]))
-    M[0:3,3] = pr[0:3]
-    return M
-
-# Transformation Matrix corresponding to a rotation about x,y or z
-def rotate(axis,ang):
-    ''' eg. T=rot('x',pi/4): rotate pi/4 rad about x axis
-    '''
-    ca=cos(ang); sa=sin(ang)
-    if axis=='x':
-        mat = ((1,0,0,0),(0,ca,-sa,0),(0,sa,ca,0),(0,0,0,1))
-    elif axis=='y':
-        mat = ((ca,0,sa,0),(0,1,0,0),(-sa,0,ca,0),(0,0,0,1))
-    elif axis=='z':
-        mat = ((ca,-sa,0,0),(sa,ca,0,0),(0,0,1,0),(0,0,0,1))
-    else:
-        print 'Axis should be: x,y or z only'
-    return mat