Rotating obstacle

In [2]:

import numpy
import sympy

import numpy import sympy

In [3]:

omega_x, omega_y, omega_z = sympy.symbols('omega_x omega_y omega_z')
c_x, c_y, c_z = sympy.symbols('c_x c_y c_z')
x, y, z, t = sympy.symbols('x y z t')

def deg2rad(deg):
    return deg * sympy.pi / 180

omega_x, omega_y, omega_z = sympy.symbols('omega_x omega_y omega_z') c_x, c_y, c_z = sympy.symbols('c_x c_y c_z') x, y, z, t = sympy.symbols('x y z t') def deg2rad(deg): return deg * sympy.pi / 180

$$ \begin{bmatrix} \Delta{x} \\ \Delta{y} \\ \Delta{z} \end{bmatrix} = R_x(\omega_x t) R_y(\omega_y t) R_z(\omega_z t) \left( \begin{bmatrix} x \\ y \\ z \end{bmatrix} - \begin{bmatrix} c_x \\ c_y \\ c_z \end{bmatrix} \right) + \begin{bmatrix} c_x \\ c_y \\ c_z \end{bmatrix} - \begin{bmatrix} x \\ y \\ z \end{bmatrix} $$

In [4]:

Rx = numpy.array([
    [1, 0, 0],
    [0, sympy.cos(omega_x*t), -sympy.sin(omega_x*t)],
    [0, sympy.sin(omega_x*t), sympy.cos(omega_x*t)]
])

Ry = numpy.array([
    [sympy.cos(omega_y*t), 0, sympy.sin(omega_y*t)],
    [0, 1, 0],
    [-sympy.sin(omega_y*t), 0, sympy.cos(omega_y*t)]
])

Rz = numpy.array([
    [sympy.cos(omega_z*t), -sympy.sin(omega_z*t), 0],
    [sympy.sin(omega_z*t), sympy.cos(omega_z*t), 0],
    [0, 0, 1]
])

R = Rx @ Ry @ Rz

c = numpy.array([c_x, c_y, c_z])
pos = numpy.array([x, y, z])
disp = R @ (pos - c) + c - pos

display(sympy.Matrix(disp))

Rx = numpy.array([ [1, 0, 0], [0, sympy.cos(omega_x*t), -sympy.sin(omega_x*t)], [0, sympy.sin(omega_x*t), sympy.cos(omega_x*t)] ]) Ry = numpy.array([ [sympy.cos(omega_y*t), 0, sympy.sin(omega_y*t)], [0, 1, 0], [-sympy.sin(omega_y*t), 0, sympy.cos(omega_y*t)] ]) Rz = numpy.array([ [sympy.cos(omega_z*t), -sympy.sin(omega_z*t), 0], [sympy.sin(omega_z*t), sympy.cos(omega_z*t), 0], [0, 0, 1] ]) R = Rx @ Ry @ Rz c = numpy.array([c_x, c_y, c_z]) pos = numpy.array([x, y, z]) disp = R @ (pos - c) + c - pos display(sympy.Matrix(disp))

$\displaystyle \left[\begin{matrix}c_{x} - x + \left(- c_{x} + x\right) \cos{\left(\omega_{y} t \right)} \cos{\left(\omega_{z} t \right)} - \left(- c_{y} + y\right) \sin{\left(\omega_{z} t \right)} \cos{\left(\omega_{y} t \right)} + \left(- c_{z} + z\right) \sin{\left(\omega_{y} t \right)}\\c_{y} - y + \left(- c_{x} + x\right) \left(\sin{\left(\omega_{x} t \right)} \sin{\left(\omega_{y} t \right)} \cos{\left(\omega_{z} t \right)} + \sin{\left(\omega_{z} t \right)} \cos{\left(\omega_{x} t \right)}\right) + \left(- c_{y} + y\right) \left(- \sin{\left(\omega_{x} t \right)} \sin{\left(\omega_{y} t \right)} \sin{\left(\omega_{z} t \right)} + \cos{\left(\omega_{x} t \right)} \cos{\left(\omega_{z} t \right)}\right) - \left(- c_{z} + z\right) \sin{\left(\omega_{x} t \right)} \cos{\left(\omega_{y} t \right)}\\c_{z} - z + \left(- c_{x} + x\right) \left(\sin{\left(\omega_{x} t \right)} \sin{\left(\omega_{z} t \right)} - \sin{\left(\omega_{y} t \right)} \cos{\left(\omega_{x} t \right)} \cos{\left(\omega_{z} t \right)}\right) + \left(- c_{y} + y\right) \left(\sin{\left(\omega_{x} t \right)} \cos{\left(\omega_{z} t \right)} + \sin{\left(\omega_{y} t \right)} \sin{\left(\omega_{z} t \right)} \cos{\left(\omega_{x} t \right)}\right) + \left(- c_{z} + z\right) \cos{\left(\omega_{x} t \right)} \cos{\left(\omega_{y} t \right)}\end{matrix}\right]$

In [5]:

def print_obstacle_displacement(omega, c):
    print("[")
    subs = {
        omega_x: deg2rad(omega[0]),
        omega_y: deg2rad(omega[1]),
        omega_z: deg2rad(omega[2]),
        c_x: c[0],
        c_y: c[1],
        c_z: c[2],
    }
    for i, d in enumerate(disp):
        expr = d.subs(subs).simplify()
        expr_str = str(expr) if expr.is_constant() else f"\"{str(expr).replace('**', '^')}\""
        print("{}{}".format(expr_str, "," if i < len(disp) - 1 else ""))
    print("]")

def print_obstacle_displacement(omega, c): print("[") subs = { omega_x: deg2rad(omega[0]), omega_y: deg2rad(omega[1]), omega_z: deg2rad(omega[2]), c_x: c[0], c_y: c[1], c_z: c[2], } for i, d in enumerate(disp): expr = d.subs(subs).simplify() expr_str = str(expr) if expr.is_constant() else f"\"{str(expr).replace('**', '^')}\"" print("{}{}".format(expr_str, "," if i < len(disp) - 1 else "")) print("]")

In [6]:

print_obstacle_displacement([0, 0, -36], [-0.35, 0, 0])
print()
print_obstacle_displacement([0, 0, 36], [0.35, 0, 0])

print_obstacle_displacement([0, 0, -36], [-0.35, 0, 0]) print() print_obstacle_displacement([0, 0, 36], [0.35, 0, 0])

[
"-x + y*sin(pi*t/5) + (x + 0.35)*cos(pi*t/5) - 0.35",
"y*cos(pi*t/5) - y - (x + 0.35)*sin(pi*t/5)",
0
]

[
"-x - y*sin(pi*t/5) + (x - 0.35)*cos(pi*t/5) + 0.35",
"y*cos(pi*t/5) - y + (x - 0.35)*sin(pi*t/5)",
0
]

In [7]:

print_obstacle_displacement([0, 360/0.525, 0], [0, 0.1, 0])

print_obstacle_displacement([0, 360/0.525, 0], [0, 0.1, 0])

[
"x*cos(3.80952380952381*pi*t) - x + z*sin(3.80952380952381*pi*t)",
"0",
"-x*sin(3.80952380952381*pi*t) + z*cos(3.80952380952381*pi*t) - z"
]

In [12]:

print_obstacle_displacement([20, 0, 0], [0, 0, 0])

print_obstacle_displacement([20, 0, 0], [0, 0, 0])

[
"0",
"y*cos(pi*t/9) - y - z*sin(pi*t/9)",
"y*sin(pi*t/9) + z*cos(pi*t/9) - z"
]

In [ ]:

---

Last update: 2023-10-03