## Coordinate System
Coordinate system in 6 Dof


```python
import warnings
#warnings.filterwarnings('ignore')
%matplotlib inline
%load_ext autoreload
%autoreload 2
```


```python
import sympy as sp
import sympy.physics.mechanics as me
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

from substitute_dynamic_symbols import substitute_dynamic_symbols, find_name, find_derivative_name, lambdify, find_derivatives

```


```python
phi,theta,psi = sp.symbols('phi theta psi')


p_x = sp.Symbol('p_x')
p_y = sp.Symbol('p_y')
p_z = sp.Symbol('p_z')


```


```python
N = me.ReferenceFrame('N')
```


```python
#M = N.orientnew('M', 'Axis', [psi, N.z])
#O = me.ReferenceFrame('O')
#O.orient(M,'Axis',[phi,M.x])
#
#S = me.ReferenceFrame('S')
#S.orient(O,'Axis',[theta,M.y])




```


```python
S = N.orientnew('S','Body',[psi,theta,phi],'ZYX')
```


```python
S.dcm(N)
```




$\displaystyle \left[\begin{matrix}\cos{\left(\psi \right)} \cos{\left(\theta \right)} & \sin{\left(\psi \right)} \cos{\left(\theta \right)} & - \sin{\left(\theta \right)}\\\sin{\left(\phi \right)} \sin{\left(\theta \right)} \cos{\left(\psi \right)} - \sin{\left(\psi \right)} \cos{\left(\phi \right)} & \sin{\left(\phi \right)} \sin{\left(\psi \right)} \sin{\left(\theta \right)} + \cos{\left(\phi \right)} \cos{\left(\psi \right)} & \sin{\left(\phi \right)} \cos{\left(\theta \right)}\\\sin{\left(\phi \right)} \sin{\left(\psi \right)} + \sin{\left(\theta \right)} \cos{\left(\phi \right)} \cos{\left(\psi \right)} & - \sin{\left(\phi \right)} \cos{\left(\psi \right)} + \sin{\left(\psi \right)} \sin{\left(\theta \right)} \cos{\left(\phi \right)} & \cos{\left(\phi \right)} \cos{\left(\theta \right)}\end{matrix}\right]$




```python
P = p_x*S.x + p_y*S.y + p_z*S.z
```


```python
P.to_matrix(N)
```




$\displaystyle \left[\begin{matrix}p_{x} \cos{\left(\psi \right)} \cos{\left(\theta \right)} + p_{y} \left(\sin{\left(\phi \right)} \sin{\left(\theta \right)} \cos{\left(\psi \right)} - \sin{\left(\psi \right)} \cos{\left(\phi \right)}\right) + p_{z} \left(\sin{\left(\phi \right)} \sin{\left(\psi \right)} + \sin{\left(\theta \right)} \cos{\left(\phi \right)} \cos{\left(\psi \right)}\right)\\p_{x} \sin{\left(\psi \right)} \cos{\left(\theta \right)} + p_{y} \left(\sin{\left(\phi \right)} \sin{\left(\psi \right)} \sin{\left(\theta \right)} + \cos{\left(\phi \right)} \cos{\left(\psi \right)}\right) + p_{z} \left(- \sin{\left(\phi \right)} \cos{\left(\psi \right)} + \sin{\left(\psi \right)} \sin{\left(\theta \right)} \cos{\left(\phi \right)}\right)\\- p_{x} \sin{\left(\theta \right)} + p_{y} \sin{\left(\phi \right)} \cos{\left(\theta \right)} + p_{z} \cos{\left(\phi \right)} \cos{\left(\theta \right)}\end{matrix}\right]$




```python
args = [
    p_x,
    p_y,
    p_z,
    phi,
    theta,
    psi,
]

rotation_lambda = sp.lambdify(args,P.to_matrix(N),modules='numpy')
```


```python
def rotate2(p,phi_=0,theta_=0,psi_=0):
    
    args = {
        'p_x':p[0],
        'p_y':p[1],
        'p_z':p[2],
        'phi':phi_,
        'theta':theta_,
        'psi':psi_
        }
    
    result = rotation_lambda(**args)
    return result.reshape(3,result.shape[2]).T
    
```


```python
import ipyvolume as ipv
import ipywidgets as widgets

class Scene:
    def __init__(self):
        
        self.figure = ipv.figure(animation=0.,animation_exponent=0,width=800,height=800)
        
        
        R = 1.
        ipv.xlim(0, R)
        ipv.ylim(0, R)
        ipv.zlim(0, R)
        ipv.style.box_off()
        
        self.load_ship_geometry()
        
        self.port = ipv.plot_mesh(self.X, self.Y_port, self.Z, wireframe=False)
        self.stbd = ipv.plot_mesh(self.X, self.Y_starboard, self.Z, wireframe=False)

        ipv.pylab.view(azimuth=180, elevation=60, distance=1)
        ipv.show()
        
        self.ui = widgets.interact(self.update, phi=(-180.,180.), theta=(-180., 180.), psi=(-180., 180.))
        
    def load_ship_geometry(self):
        
        self.N = 10
        L = 1
        B = 0.1
        T = 0.05
        
        x = np.linspace(0,L,self.N)
        y = np.linspace(0,B,self.N)
        z = np.linspace(0,T,self.N)
        
        X,Y = np.meshgrid(x,y)
        Z=T-np.array(Y)
        Y*=(L-X)**(0.4)
        
        self.X = X-L/2
        self.Z = Z
        self.Y_port = Y
        self.Y_starboard = -Y
        
    def update(self, phi, theta, psi):
        print("update with", phi, theta, psi)
        
        angles = np.deg2rad([phi, theta, psi])
        
        X = self.X.flatten()
        Y = self.Y_starboard.flatten()
        Z = self.Z.flatten()
        result = rotate2(p=[X,Y,Z],phi_=angles[0],theta_=angles[1],psi_=angles[2])
        X_ = result[:,0]
        Y_ = result[:,1]
        Z_ = result[:,2]
        self.stbd.x = X_
        self.stbd.y = Y_
        self.stbd.z = Z_
        
        X = self.X.flatten()
        Y = self.Y_port.flatten()
        Z = self.Z.flatten()
        result = rotate2(p=[X,Y,Z],phi_=angles[0],theta_=angles[1],psi_=angles[2])
        X_ = result[:,0]
        Y_ = result[:,1]
        Z_ = result[:,2]
        self.port.x = X_
        self.port.y = Y_
        self.port.z = Z_
               
                

scene = Scene()
```


    VBox(children=(Figure(animation=0.0, animation_exponent=0.0, camera=PerspectiveCamera(fov=46.0, position=(6.12…



    interactive(children=(FloatSlider(value=0.0, description='phi', max=180.0, min=-180.0), FloatSlider(value=0.0,…



```python

```

dd41921d6fe57d29f1c63ea1896a11448ace73a0
ipynb
Jupyter Notebook
coordinate_system.ipynb
axelande/rigidbodysimulator
a87c3eb3b7978ef01efca15e66a6de6518870cd8
MIT
coordinate_system.ipynb
axelande/rigidbodysimulator
a87c3eb3b7978ef01efca15e66a6de6518870cd8
MIT
2020-10-26T19:47:02.000Z
2020-10-26T19:47:02.000Z
coordinate_system.ipynb
axelande/rigidbodysimulator
a87c3eb3b7978ef01efca15e66a6de6518870cd8
MIT
2020-10-26T09:17:00.000Z
2020-10-26T09:17:00.000Z
Qwen/Qwen-72B
1. YES
2. YES