Python file with (symbolic and numerical) definitions for transformation matrices.

xforms.py

import numpy as np
import sympy as sp

class SO3:
    def rotx(theta):
        c, s = np.cos(theta), np.sin(theta)
        return np.matrix([
            [1, 0, 0],
            [0, c, -s],
            [0, s, c]
        ])
        
    def roty(theta):
        c, s = np.cos(theta), np.sin(theta)
        return np.matrix([
            [c, 0, s],
            [0, 1, 0],
            [-s, 0, c]
        ])
        
    def rotz(theta):
        c, s = np.cos(theta), np.sin(theta)
        return np.matrix([
            [c, -s, 0],
            [s, c, 0],
            [0, 0, 1]
        ])
        
class SE3:
    def rotx(theta):
        c, s = np.cos(theta), np.sin(theta)
        return np.matrix([
            [1, 0, 0, 0],
            [0, c, -s, 0],
            [0, s, c, 0],
            [0, 0, 0, 1]
        ])
        
    def roty(theta):
        c, s = np.cos(theta), np.sin(theta)
        return np.matrix([
            [c, 0, s, 0],
            [0, 1, 0, 0],
            [-s, 0, c, 0],
            [0, 0, 0, 1]
        ])
        
    def rotz(theta):
        c, s = np.cos(theta), np.sin(theta)
        return np.matrix([
            [c, -s, 0, 0],
            [s, c, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])

    def trax(x):
        return np.matrix([
            [1, 0, 0, x],
            [0, 1, 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])
        
    def tray(y):
        return np.matrix([
            [1, 0, 0, 0],
            [0, 1, 0, y],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])
        
    def traz(z):
        return np.matrix([
            [1, 0, 0, 0],
            [0, 1, 0, 0],
            [0, 0, 1, z],
            [0, 0, 0, 1]
        ])

    def getOrientationMtx(mtx):
        return mtx[:3, :3]

    def getPositionMtx(mtx):
        return mtx[:3, 3]

# Use * to make parameters keyword params
def dhMatrix(*, a, alpha, d, theta):    
    return np.matrix([
        [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)],
        [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]   
    ],)

# Symbolic math helpers
class Symbolic:
    class SO3:
        def rotx(theta):
            c, s = sp.cos(theta), sp.sin(theta)
            return sp.Matrix([
                [1, 0, 0],
                [0, c, -s],
                [0, s, c]
            ])

        def roty(theta):
            c, s = sp.cos(theta), sp.sin(theta)
            return sp.Matrix([
                [c, 0, s],
                [0, 1, 0],
                [-s, 0, c]
            ])

        def rotz(theta):
            c, s = sp.cos(theta), sp.sin(theta)
            return sp.Matrix([
                [c, -s, 0],
                [s, c, 0],
                [0, 0, 1]
            ])

    class SE3:
        def rotx(theta):
            c, s = sp.cos(theta), sp.sin(theta)
            return sp.Matrix([
                [1, 0, 0, 0],
                [0, c, -s, 0],
                [0, s, c, 0],
                [0, 0, 0, 1]
            ])

        def roty(theta):
            c, s = sp.cos(theta), sp.sin(theta)
            return sp.Matrix([
                [c, 0, s, 0],
                [0, 1, 0, 0],
                [-s, 0, c, 0],
                [0, 0, 0, 1]
            ])

        def rotz(theta):
            c, s = sp.cos(theta), sp.sin(theta)
            return sp.Matrix([
                [c, -s, 0, 0],
                [s, c, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]
            ])

        def trax(x):
            return sp.Matrix([
                [1, 0, 0, x],
                [0, 1, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]
            ])

        def tray(y):
            return sp.Matrix([
                [1, 0, 0, 0],
                [0, 1, 0, y],
                [0, 0, 1, 0],
                [0, 0, 0, 1]
            ])

        def traz(z):
            return sp.Matrix([
                [1, 0, 0, 0],
                [0, 1, 0, 0],
                [0, 0, 1, z],
                [0, 0, 0, 1]
            ])
            
        def getOrientationMtx(mtx):
            return mtx[:3, :3]

        def getPositionMtx(mtx):
            return mtx[:3, 3]

    # Use * to make parameters keyword params
    def dhMatrix(*, a, alpha, d, theta):
        A = sp.Matrix([
            [sp.cos(theta), -sp.sin(theta)*sp.cos(alpha), sp.sin(theta)*sp.sin(alpha), a*sp.cos(theta)],
            [sp.sin(theta), sp.cos(theta)*sp.cos(alpha), -sp.cos(theta)*sp.sin(alpha), a*sp.sin(theta)],
            [0, sp.sin(alpha), sp.cos(alpha), d],
            [0, 0, 0, 1]
        ])
        return A