I wrote a module called geo.py that performs 3D geometric operations such as vectors, rotation matrices, and coordinate transformations.

This ipynb file shows a basic usage of the geo module.

This was run under the following conditions, - ProBook 474s, メモリ：8 GB，CPU：Core™ i5-3230M，OS: Ubuntu 20.04 - jupyter notebook，python3

The file is matrix_vs_quaternion.ipynb.

Note: geo.py imports the math module in the form “from math import *”.

geo.py defines the VECTOR class, MATRIX class, QUATERNION class, and FRAME class. The class names are all capitalized to avoid name collisions with other similar classes.

from geo import *

VECTOR class is defined as a subclass of list.

class VECTOR(list) :
    def __init__(self, x=0.0, y=0.0, z=0.0, vec=[]) :  # vec is provided  to create a copy of a VECTOR.
    def __repr__(self) : 
    def dot(self, other) : # inner product
    def __neg__(self) :  # negative vector
    def __add__(self, other) : # addition
    def __radd__(self, other) : # addition to avoid list concatenation
    def __sub__(self, other) : # subtraction
    def __mul__(self, other) : # cross product
    def __rmul__(self, other) : # multiplication with scalar and MATRIX(this is defined in MATRIX)
    def __abs__(self) : # absolute value
    def normalize(self) : # normalizetion

v1=VECTOR(1,2,3)
v2=VECTOR(y=3)
print(v1)
print(v2)

v:[1.0, 2.0, 3.0]
v:[0.0, 3.0, 0.0]

v1+v2

v:[1.0, 5.0, 3.0]

v1-v2

v:[1.0, -1.0, 3.0]

v1.dot(v2)

6.0

v1 * v2

v:[-9.0, 0.0, 3.0]

3*v1

v:[3.0, 6.0, 9.0]

v1*3  

---------------------------------------------------------------------------

TypeError                                 Traceback (most recent call last)

<ipython-input-20-e77cfd6fc54c> in <module>
----> 1 v1*3


~/openrtm/Robots/core/geo.py in __mul__(self, other)
     24   def __mul__(self, other) : # cross protuct
     25     if not isinstance(other, VECTOR) :
---> 26        raise TypeError('error')
     27     tmp = VECTOR();
     28     tmp[0] = self[1]*other[2] - self[2]*other[1]


TypeError: error

-v1

v:[-1.0, -2.0, -3.0]

abs(v1)

3.7416573867739413

v3=v1.normalize()
print("v1: ", v1)
print("v3: ", v3)
print("abs(v3): ", abs(v3))

v1:  v:[1.0, 2.0, 3.0]
v3:  v:[0.2672612419124244, 0.5345224838248488, 0.8017837257372732]
abs(v3):  1.0

a=VECTOR(0.866,0.5,0)
b=VECTOR(0.707,0.707,0)
angle_rad = acos(a.dot(b)/abs(a)/abs(b))
angle_deg = 180*angle_rad/pi
print(angle_deg)

14.999272219172607

a wrong answer

a=VECTOR(1,0,0)
b=VECTOR(-0.707,-0.707,0)
angle_rad = acos(a.dot(b)/abs(a)/abs(b))
angle_deg = 180*angle_rad/pi
print(angle_deg)

135.0

the right answer

x=VECTOR(1,0,0)
y=VECTOR(0,1,0)
b=VECTOR(-0.707,-0.707,0)
co=x.dot(b)
si=x.dot(b)
angle_rad = atan2(si,co)
angle_deg = 180*angle_rad/pi
print(angle_deg)

-135.0

MATRIX class is defined as subclass of list. It is a list of 3 lists(rows of the matrix).

MATRIX here is not a general matrix. It is an orthogonal matrix which represents rotation operation (exclude mirror operation) or orientation of a right-hand coordinate system.

So it does not have real inverse operation. Here it is just same as transpose operation.

class MATRIX(list) :
    def __init__(self, mat=[], a=None, b=None, c=None, angle=None, axis=None) : # a,b,c are rotations around x,y,z axes
    def __repr__(self) :
    def col(self, idx, arg=[]) : # extraction of column VECTOR
    def row(self, idx, arg=[]) : # extraction of row VECTOR
    def rot_axis(self) : # exraction of rotation angle and axis
    def quaternion(self):  # quaternion for rotation
    def abc(self) : # extraction of alpha, beta, gamma, rotation around x,y,z
    def rpy(self) : # extraction of roll, pitch, yaw, rotation around x,y,z
    def trans(self) : # transposition
    def __neg__(self) : # inverse, ie., transposition
    def __mul__(self, other) : # multiplication with MATRIX or VECTOR
 

generation of a rotation matrix which rotate pi/4 around x-axis.

R1=MATRIX(a=pi/4)
print(R1)

m:[[1.0, 0.0, 0.0], [0.0, 0.7071067811865476, -0.7071067811865475], [0.0, 0.7071067811865475, 0.7071067811865476]]

and rotate pi/6 around y-axis and rotate pi/3 around z-axis

R2=MATRIX(b=pi/6)
R3=MATRIX(c=pi/3)

R123=R1*R2*R3
R123

m:[[0.43301270189221946, -0.75, 0.49999999999999994], [0.7891491309924314, 0.04736717274537666, -0.6123724356957945], [0.4355957403991576, 0.6597396084411711, 0.6123724356957946]]

R1*VECTOR(0,1,0)

v:[0.0, 0.7071067811865476, 0.7071067811865475]

R123.abc()

[0.7853981633974482, 0.5235987755982988, 1.0471975511965976]

R123.rpy()

[0.8226160330164501, -0.45069999992702, 1.0689453418473993]

(R3*R2*R1).rpy()

[0.7853981633974482, 0.5235987755982988, 1.0471975511965976]

R4=MATRIX(axis=VECTOR(1,1,0),angle=pi/4)
R4

m:[[0.8535533905932737, 0.14644660940672619, 0.4999999999999999], [0.14644660940672619, 0.8535533905932737, -0.4999999999999999], [-0.4999999999999999, 0.4999999999999999, 0.7071067811865476]]

R4.rot_axis()

[0.7853981633974483, v:[0.7071067811865475, 0.7071067811865475, 0.0]]

print(R1.trans())
R1*R1.trans()

m:[[1.0, 0.0, 0.0], [0.0, 0.7071067811865476, 0.7071067811865475], [0.0, -0.7071067811865475, 0.7071067811865476]]





m:[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]

print(-R1)
R1*(-R1)

m:[[1.0, 0.0, 0.0], [0.0, 0.7071067811865476, 0.7071067811865475], [0.0, -0.7071067811865475, 0.7071067811865476]]





m:[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]

R1.trans().rot_axis()

[0.7853981633974483, v:[-0.9999999999999999, 0.0, 0.0]]

R2.col(0)

v:[0.8660254037844387, 0.0, -0.49999999999999994]

R2.row(0)

v:[0.8660254037844387, 0.0, 0.49999999999999994]

R1.quaternion()

q:(0.9238795325112867, v:[0.3826834323650897, 0.0, 0.0])

QUATERNION class has a real part and an imaginary part. The imaginary part is defined as a VECTOR.

Quaternion itself is not only for these operations, it is a kind of number. But QUATERNION here is mainly used for representing rotation operation or orientation of a coordinate system. So only the methods necessary for them are implemented.

class QUATERNION(object) : # quaternion mainly for rotation
    def __init__(self,w_v=None, a=None, b=None, c=None, angle=None, axis=None) : w_v is a list [w,v]
    def __repr__(self) :
    def __mul__(self, other) :
    def __neg__(self) :
    def __abs__(self) :
    def normalize(self) :
    def inv(self) :
    def conjugate(self) :
    def matrix(self) :

q1 = QUATERNION(a=pi/4)
q1

q:(0.9238795325112867, v:[0.3826834323650898, 0.0, 0.0])

q2 = QUATERNION(b=pi/6)
q3 = QUATERNION(c=pi/3)

you shouod give arguments as [cos(th/2), sin(th/2)*axis.normalize()]

co=cos((pi/4)/2)
si=sin((pi/4)/2)
q4=QUATERNION([co, si*VECTOR(1,0,0)])
q4

q:(0.9238795325112867, v:[0.3826834323650898, 0.0, 0.0])

axis does not have to be normalized.

q5=QUATERNION(angle=pi/4, axis=VECTOR(2,0,0))
q5

q:(0.9238795325112867, v:[0.3826834323650898, 0.0, 0.0])

q123=q1*q2*q3

q123.matrix()

m:[[0.43301270189221946, -0.7500000000000001, 0.5], [0.7891491309924316, 0.04736717274537666, -0.6123724356957946], [0.4355957403991576, 0.6597396084411712, 0.6123724356957947]]

R123

m:[[0.43301270189221946, -0.75, 0.49999999999999994], [0.7891491309924314, 0.04736717274537666, -0.6123724356957945], [0.4355957403991576, 0.6597396084411711, 0.6123724356957946]]

q1*q1.conjugate()

q:(1.0, v:[0.0, 0.0, 0.0])

inverse for any QUATERNION

q4=QUATERNION([2,VECTOR(1,2,3)])
q4

q:(2, v:[1.0, 2.0, 3.0])

q4*q4.inv()

q:(4.242640687119286, v:[0.0, 0.0, 0.0])

abs(q4)

4.242640687119285

q5=q4.normalize()

q4

q:(2, v:[1.0, 2.0, 3.0])

q5

q:(0.47140452079103173, v:[0.23570226039551587, 0.47140452079103173, 0.7071067811865476])

q1*QUATERNION([0,VECTOR(0,1,0)])*q1.conjugate()

q:(0.0, v:[0.0, 0.7071067811865475, 0.7071067811865476])

FRAME has a MATRIX and a VECTOR which represents orientation and position of coordinate system. It also represets transformation of coordinate system.

class FRAME(object) :
    def __init__(self, frm=[], mat=[], vec=[], xyzabc=[], xyzrpy=[]) :
    def __repr__(self) :
    def xyzabc(self) :
    def xyzrpy(self) :
    def __neg__(self) : # inverse
    def __mul__(self, other) : # multiplication with frame or positional vector
 

F1 = FRAME(mat=R123,vec=v1)
F1

f:(m:[[0.43301270189221946, -0.75, 0.49999999999999994], [0.7891491309924314, 0.04736717274537666, -0.6123724356957945], [0.4355957403991576, 0.6597396084411711, 0.6123724356957946]],v:[1.0, 2.0, 3.0])

F2=FRAME(xyzabc=[1,2,3,pi/4,pi/6,pi/3])
F2

f:(m:[[0.43301270189221946, -0.75, 0.49999999999999994], [0.7891491309924314, 0.04736717274537666, -0.6123724356957945], [0.4355957403991576, 0.6597396084411711, 0.6123724356957946]],v:[1.0, 2.0, 3.0])

F3=FRAME(xyzrpy=[1,2,4,pi/4,pi/6,pi/3])
F3

f:(m:[[0.43301270189221946, -0.4355957403991577, 0.7891491309924313], [0.75, 0.6597396084411711, -0.04736717274537655], [-0.49999999999999994, 0.6123724356957945, 0.6123724356957946]],v:[1.0, 2.0, 4.0])

F1.xyzabc()

[1.0, 2.0, 3.0, 0.7853981633974482, 0.5235987755982988, 1.0471975511965976]

F3.xyzabc()

[1.0, 2.0, 4.0, 0.07719655674229194, 0.9094224366744116, 0.7883719209238913]

F1.xyzrpy()

[1.0, 2.0, 3.0, 0.8226160330164501, -0.45069999992702, 1.0689453418473993]

F3.xyzrpy()

[1.0, 2.0, 4.0, 0.7853981633974482, 0.5235987755982988, 1.0471975511965976]

product of frames represents the coordinate transformation.

F1*F2

f:(m:[[-0.18656397804474475, -0.03041510175761164, 0.9819718955658528], [0.11234453608956696, -0.9936245501365428, -0.009431785449894092], [0.9759982516925634, 0.10855954564674164, 0.18879151925346918]],v:[1.4330127018922192, 1.0467661693958012, 6.592192264368883])

product of a frame and a vector represents the coordinate transformation of the position vector.

F1*v2 = R123*v2+v1

F1*v2

v:[-1.25, 2.14210151823613, 4.979218825323513]

R123*v2

v:[-2.25, 0.14210151823613, 1.9792188253235132]

R123*v2+v1

v:[-1.25, 2.14210151823613, 4.979218825323513]