CER Demo 1

transf_demo.py

from functools import reduce
from math import cos, sin, pi
import numpy as np


def rad(theta):
    """
    Grad in Radiane umwandeln und zurückgeben
    """
    return theta * pi / 180

def Rz(theta):
    """
    Rotationmatrix um die z-Achse berechnen
    """
    return np.array(((cos(theta), -sin(theta), 0), (sin(theta), cos(theta), 0), (0, 0, 1)))

def gen_T(Rot, r):
    """
    Transformationsmatrix aus Rotationsmatrix Rot und
    Vektor r konstruieren und zurückgeben
    """
    return np.array(((Rot, r), (0, 1)), dtype=object)

def single_transf(T0, T1):
    """
    T.matrizen multiplizieren
    """
    Rot0, Rot1 = T0[0][0], T1[0][0]
    r0, r1 = T0[0][1], T1[0][1]
    return gen_T((Rot0 @ Rot1), (Rot0 @ r1 + r0))

def transf(T_list):
    """
    Transoformation belieber Länge durchführen
    """
    i_matrix = gen_T(np.identity(3), np.array([[0,0,0]]).T)
    return reduce(single_transf, T_list, i_matrix)

# einige Größen initialisieren
l1 = np.array([[40.0,0,0]]).T
l2 = np.array([[20.0,0,0]]).T
l3 = np.array([[30.0,0,0]]).T
q1 = rad(60)
q2 = rad(225)
q3 = rad(90)

# Rotations- und Transformationsmatrizen berechnen
R01, R12, R23 = Rz(q1), Rz(q2), Rz(q3)
T01 = gen_T(R01, R01 @ l1)
T12 = gen_T(R12, R12 @ l2)
T23 = gen_T(R23, R23 @ l3)

# Transformation durchführen
transf([T01, T12, T23])