====== geo.pyの基本的な使い方 ======

====== geo_basic ======

I wrote a module called [[./geo.py|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|matrix_vs_quaternion.ipynb]].

===== import module =====

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.

<code python>
from geo import *
</code>
===== VECTOR =====

VECTOR class is defined as a subclass of list.

<code python>
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
</code>
==== generationg VECTORs ====

<code python>
v1=VECTOR(1,2,3)
v2=VECTOR(y=3)
print(v1)
print(v2)
</code>
<code>
v:[1.0, 2.0, 3.0]
v:[0.0, 3.0, 0.0]
</code>
===== addition and subtraction =====

<code python>
v1+v2
</code>
<code>
v:[1.0, 5.0, 3.0]
</code>
<code python>
v1-v2
</code>
<code>
v:[1.0, -1.0, 3.0]
</code>
==== inner product ====

<code python>
v1.dot(v2)
</code>
<code>
6.0
</code>
==== cross product ====

<code python>
v1 * v2
</code>
<code>
v:[-9.0, 0.0, 3.0]
</code>
==== scalar multiplication ====

<code python>
3*v1
</code>
<code>
v:[3.0, 6.0, 9.0]
</code>
<code python>
v1*3  
</code>
<code>
---------------------------------------------------------------------------

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
</code>
==== negatification ====

<code python>
-v1
</code>
<code>
v:[-1.0, -2.0, -3.0]
</code>
==== absolute value ====

<code python>
abs(v1)
</code>
<code>
3.7416573867739413
</code>
==== nomalization ====

<code python>
v3=v1.normalize()
print("v1: ", v1)
print("v3: ", v3)
print("abs(v3): ", abs(v3))
</code>
<code>
v1:  v:[1.0, 2.0, 3.0]
v3:  v:[0.2672612419124244, 0.5345224838248488, 0.8017837257372732]
abs(v3):  1.0
</code>
==== exercises ====

=== Find the angle between VECTOR(0.866, 0.5, 0) and VECTOR(0.707, 0.707, 0). ===

<code python>
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)
</code>
<code>
14.999272219172607
</code>
=== Find the rotation angle around the z-axis of VECTOR(-0.707,-0.707, 0) from VECTOR(1,0,0) . ===

a wrong answer

<code python>
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)
</code>
<code>
135.0
</code>
the right answer

<code python>
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)
</code>
<code>
-135.0
</code>
===== MATRIX =====

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.

<code python>
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
    
</code>
==== generation of MATRIXs (matrices) ====

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

<code python>
R1=MATRIX(a=pi/4)
print(R1)
</code>
<code>
m:[[1.0, 0.0, 0.0], [0.0, 0.7071067811865476, -0.7071067811865475], [0.0, 0.7071067811865475, 0.7071067811865476]]
</code>
and rotate pi/6 around y-axis and rotate pi/3 around z-axis

<code python>
R2=MATRIX(b=pi/6)
R3=MATRIX(c=pi/3)
</code>
==== product of matrices ====

<code python>
R123=R1*R2*R3
R123
</code>
<code>
m:[[0.43301270189221946, -0.75, 0.49999999999999994], [0.7891491309924314, 0.04736717274537666, -0.6123724356957945], [0.4355957403991576, 0.6597396084411711, 0.6123724356957946]]
</code>
==== product of a matrix and a vector, ie. , coordinate transformation or rotation of the vector ====

<code python>
R1*VECTOR(0,1,0)
</code>
<code>
v:[0.0, 0.7071067811865476, 0.7071067811865475]
</code>
==== extraction of abc(alph, beta, gamma Euler angles) ====

<code python>
R123.abc()
</code>
<code>
[0.7853981633974482, 0.5235987755982988, 1.0471975511965976]
</code>
==== extraction of rpy(roll, pitch, yaw Euler angles) ====

<code python>
R123.rpy()
</code>
<code>
[0.8226160330164501, -0.45069999992702, 1.0689453418473993]
</code>
<code python>
(R3*R2*R1).rpy()
</code>
<code>
[0.7853981633974482, 0.5235987755982988, 1.0471975511965976]
</code>
==== generation of MATRIX with rotation axis and angle (rodrigues) ====

<code python>
R4=MATRIX(axis=VECTOR(1,1,0),angle=pi/4)
R4
</code>
<code>
m:[[0.8535533905932737, 0.14644660940672619, 0.4999999999999999], [0.14644660940672619, 0.8535533905932737, -0.4999999999999999], [-0.4999999999999999, 0.4999999999999999, 0.7071067811865476]]
</code>
==== extraction of rotation axis and angle (rodrigues) ====

<code python>
R4.rot_axis()
</code>
<code>
[0.7853981633974483, v:[0.7071067811865475, 0.7071067811865475, 0.0]]
</code>
==== transpose or inverse of MATRIX ====

<code python>
print(R1.trans())
R1*R1.trans()
</code>
<code>
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]]
</code>
<code python>
print(-R1)
R1*(-R1)
</code>
<code>
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]]
</code>
<code python>
R1.trans().rot_axis()
</code>
<code>
[0.7853981633974483, v:[-0.9999999999999999, 0.0, 0.0]]
</code>
==== extraction of column and row as a VECTOR ====

<code python>
R2.col(0)
</code>
<code>
v:[0.8660254037844387, 0.0, -0.49999999999999994]
</code>
<code python>
R2.row(0)
</code>
<code>
v:[0.8660254037844387, 0.0, 0.49999999999999994]
</code>
==== MATRIX to QUATERNION ====

<code python>
R1.quaternion()
</code>
<code>
q:(0.9238795325112867, v:[0.3826834323650897, 0.0, 0.0])
</code>

===== QUATERNION =====

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.

<code python>
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) :
</code>
==== generation of QUATERNION ====

<code python>
q1 = QUATERNION(a=pi/4)
q1
</code>
<code>
q:(0.9238795325112867, v:[0.3826834323650898, 0.0, 0.0])
</code>
<code python>
q2 = QUATERNION(b=pi/6)
q3 = QUATERNION(c=pi/3)
</code>
=== generation of QUATERNION with [w, v] ===

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

<code python>
co=cos((pi/4)/2)
si=sin((pi/4)/2)
q4=QUATERNION([co, si*VECTOR(1,0,0)])
q4
</code>
<code>
q:(0.9238795325112867, v:[0.3826834323650898, 0.0, 0.0])
</code>
=== generation of QUATERNION with rotation angle and axis ===

axis does not have to be normalized.

<code python>
q5=QUATERNION(angle=pi/4, axis=VECTOR(2,0,0))
q5
</code>
<code>
q:(0.9238795325112867, v:[0.3826834323650898, 0.0, 0.0])
</code>
==== product of QUATERNION ====

<code python>
q123=q1*q2*q3
</code>
==== QUATERNION to MATRIX ====

<code python>
q123.matrix()
</code>
<code>
m:[[0.43301270189221946, -0.7500000000000001, 0.5], [0.7891491309924316, 0.04736717274537666, -0.6123724356957946], [0.4355957403991576, 0.6597396084411712, 0.6123724356957947]]
</code>
<code python>
R123
</code>
<code>
m:[[0.43301270189221946, -0.75, 0.49999999999999994], [0.7891491309924314, 0.04736717274537666, -0.6123724356957945], [0.4355957403991576, 0.6597396084411711, 0.6123724356957946]]
</code>
==== conjugate of QUATERNION, which is inverse of rotational quaternion. ====

<code python>
q1*q1.conjugate()
</code>
<code>
q:(1.0, v:[0.0, 0.0, 0.0])
</code>
==== inverse of QUATERNION ====

inverse for any QUATERNION

<code python>
q4=QUATERNION([2,VECTOR(1,2,3)])
q4
</code>
<code>
q:(2, v:[1.0, 2.0, 3.0])
</code>
<code python>
q4*q4.inv()
</code>
<code>
q:(4.242640687119286, v:[0.0, 0.0, 0.0])
</code>
==== absolute value and nomalization ====

<code python>
abs(q4)
</code>
<code>
4.242640687119285
</code>
<code python>
q5=q4.normalize()
</code>
<code python>
q4
</code>
<code>
q:(2, v:[1.0, 2.0, 3.0])
</code>
<code python>
q5
</code>
<code>
q:(0.47140452079103173, v:[0.23570226039551587, 0.47140452079103173, 0.7071067811865476])
</code>
==== rotattion of vector ====

<code python>
q1*QUATERNION([0,VECTOR(0,1,0)])*q1.conjugate()
</code>
<code>
q:(0.0, v:[0.0, 0.7071067811865475, 0.7071067811865476])
</code>
===== FRAME =====

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

<code python>
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
    
</code>
==== generation of FRAME ====

<code python>
F1 = FRAME(mat=R123,vec=v1)
F1
</code>
<code>
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])
</code>
<code python>
F2=FRAME(xyzabc=[1,2,3,pi/4,pi/6,pi/3])
F2
</code>
<code>
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])
</code>
<code python>
F3=FRAME(xyzrpy=[1,2,4,pi/4,pi/6,pi/3])
F3
</code>
<code>
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])
</code>
==== extraction of xyzabc ====

<code python>
F1.xyzabc()
</code>
<code>
[1.0, 2.0, 3.0, 0.7853981633974482, 0.5235987755982988, 1.0471975511965976]
</code>
<code python>
F3.xyzabc()
</code>
<code>
[1.0, 2.0, 4.0, 0.07719655674229194, 0.9094224366744116, 0.7883719209238913]
</code>
==== extraction of xyzrpy ====

<code python>
F1.xyzrpy()
</code>
<code>
[1.0, 2.0, 3.0, 0.8226160330164501, -0.45069999992702, 1.0689453418473993]
</code>
<code python>
F3.xyzrpy()
</code>
<code>
[1.0, 2.0, 4.0, 0.7853981633974482, 0.5235987755982988, 1.0471975511965976]
</code>
==== product of frames ====

product of frames represents the coordinate transformation.

<code python>
F1*F2
</code>
<code>
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])
</code>
==== product of a frame and a vector ====

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

F1*v2 = R123*v2+v1

<code python>
F1*v2
</code>
<code>
v:[-1.25, 2.14210151823613, 4.979218825323513]
</code>
<code python>
R123*v2
</code>
<code>
v:[-2.25, 0.14210151823613, 1.9792188253235132]
</code>
<code python>
R123*v2+v1
</code>
<code>
v:[-1.25, 2.14210151823613, 4.979218825323513]
</code>
<code python>

</code>