# 3 Degrees Of Freedome (DOF)


```python
%matplotlib inline
import sympy as sym
import sympy.physics.mechanics as me
from sympy import init_printing
init_printing(use_latex='mathjax')

from pydy.system import System
import numpy as np
import matplotlib.pyplot as plt

```




```python
x0,y0,psi,x,y,beta,x,y,u,v,r,V = me.dynamicsymbols(r'x0 y0 \psi x y \beta x y u v r V')
m,Izz,t = sym.symbols(r'm Izz t')
```


```python
earth_frame = me.ReferenceFrame(name = 'E')
ship_frame = me.ReferenceFrame(name = 'S')
ship_frame.orient(parent = earth_frame,rot_type='Axis',amounts = (psi,earth_frame.z))
#ship_frame.set_ang_vel(otherframe=earth_frame,value = r*earth_frame.z)
```


```python
ship_frame.dcm(otherframe=earth_frame)
```




$$\left[\begin{matrix}\cos{\left (\psi{\left (t \right )} \right )} & \sin{\left (\psi{\left (t \right )} \right )} & 0\\- \sin{\left (\psi{\left (t \right )} \right )} & \cos{\left (\psi{\left (t \right )} \right )} & 0\\0 & 0 & 1\end{matrix}\right]$$




```python
O0 = me.Point('O0')
O0.set_vel(frame = earth_frame,value = 0)

O = me.Point('O')
#O.set_pos(otherpoint=O0,value = x0*earth_frame.x + y0*earth_frame.y)
O.set_pos(otherpoint=O0,value = x*ship_frame.x + y*ship_frame.y)

O.set_vel(frame = ship_frame,value = u*ship_frame.x + v*ship_frame.y)

O.v1pt_theory(otherpoint=O0,outframe=earth_frame,interframe=ship_frame)
```




$$(u - y \dot{\psi})\mathbf{\hat{s}_x} + (v + x \dot{\psi})\mathbf{\hat{s}_y}$$




```python
O.pos_from(O0)
```




$$x\mathbf{\hat{s}_x} + y\mathbf{\hat{s}_y}$$




```python
O.vel(earth_frame).express(earth_frame)
```




$$(\left(u - y \dot{\psi}\right) \operatorname{cos}\left(\psi\right) - \left(v + x \dot{\psi}\right) \operatorname{sin}\left(\psi\right))\mathbf{\hat{e}_x} + (\left(u - y \dot{\psi}\right) \operatorname{sin}\left(\psi\right) + \left(v + x \dot{\psi}\right) \operatorname{cos}\left(\psi\right))\mathbf{\hat{e}_y}$$




```python
O.acc(earth_frame).express(earth_frame)
```




$$(- \left(\left(u - y \dot{\psi}\right) \dot{\psi} + x \ddot{\psi} + \dot{\psi} \dot{x} + \dot{v}\right) \operatorname{sin}\left(\psi\right) + \left(- \left(v + x \dot{\psi}\right) \dot{\psi} - y \ddot{\psi} - \dot{\psi} \dot{y} + \dot{u}\right) \operatorname{cos}\left(\psi\right))\mathbf{\hat{e}_x} + (\left(\left(u - y \dot{\psi}\right) \dot{\psi} + x \ddot{\psi} + \dot{\psi} \dot{x} + \dot{v}\right) \operatorname{cos}\left(\psi\right) + \left(- \left(v + x \dot{\psi}\right) \dot{\psi} - y \ddot{\psi} - \dot{\psi} \dot{y} + \dot{u}\right) \operatorname{sin}\left(\psi\right))\mathbf{\hat{e}_y}$$




```python
kinematical_differential_equations = [u - x.diff(),
                                      v - y.diff(),
                                      r - psi.diff()]
coordinates = [x,y,psi,]
speeds = [u,v,r,]
```


```python
kane = me.KanesMethod(earth_frame,coordinates, speeds,kinematical_differential_equations)
```


```python
forces = 1*ship_frame.x
mass = me.Particle('m', O, m)
```


```python
inertia_dyadic = me.inertia(ship_frame, ixx = 0, iyy = 0, izz = Izz)
inertia_dyadic
```




$$Izz\mathbf{\hat{s}_z}\otimes \mathbf{\hat{s}_z}$$




```python
inertia = (inertia_dyadic, O)
```


```python
ship = me.RigidBody('ship',masscenter=O,frame = ship_frame,mass = mass,inertia=inertia)
```


```python
fr, frstar = kane.kanes_equations(bodies = [ship],loads = [(O, forces)])
```


```python
M = kane.mass_matrix_full 
M
```




$$\left[\begin{matrix}1 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0\\0 & 0 & 0 & m & 0 & - m y{\left (t \right )}\\0 & 0 & 0 & 0 & m & m x{\left (t \right )}\\0 & 0 & 0 & - m y{\left (t \right )} & m x{\left (t \right )} & Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\end{matrix}\right]$$




```python
f = kane.forcing_full
f
```




$$\left[\begin{matrix}u{\left (t \right )}\\v{\left (t \right )}\\r{\left (t \right )}\\- m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) + 1\\- m \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right)\\m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) y{\left (t \right )} - m \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right) x{\left (t \right )} - y{\left (t \right )}\end{matrix}\right]$$




```python
(M.inv() * f)
```




$$\left[\begin{matrix}u{\left (t \right )}\\v{\left (t \right )}\\r{\left (t \right )}\\\frac{m^{3} \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right) x{\left (t \right )} y{\left (t \right )}}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)} + \frac{m^{2} \left(m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) y{\left (t \right )} - m \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right) x{\left (t \right )} - y{\left (t \right )}\right) y{\left (t \right )}}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)} + \frac{\left(- m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) + 1\right) \left(- m^{3} x^{2}{\left (t \right )} + m^{3} y^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)\right)}{m \left(- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)\right)}\\- \frac{m^{2} \left(- m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) + 1\right) x{\left (t \right )} y{\left (t \right )}}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)} - \frac{m^{2} \left(m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) y{\left (t \right )} - m \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right) x{\left (t \right )} - y{\left (t \right )}\right) x{\left (t \right )}}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)} - \frac{m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right) \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right)}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)}\\\frac{m^{3} \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right) x{\left (t \right )}}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)} + \frac{m^{2} \left(- m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) + 1\right) y{\left (t \right )}}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)} + \frac{m^{2} \left(m \left(- \left(r{\left (t \right )} x{\left (t \right )} + v{\left (t \right )}\right) r{\left (t \right )} - r{\left (t \right )} v{\left (t \right )}\right) y{\left (t \right )} - m \left(\left(- r{\left (t \right )} y{\left (t \right )} + u{\left (t \right )}\right) r{\left (t \right )} + r{\left (t \right )} u{\left (t \right )}\right) x{\left (t \right )} - y{\left (t \right )}\right)}{- m^{3} x^{2}{\left (t \right )} + m \left(- m^{2} y^{2}{\left (t \right )} + m \left(Izz + m \left(x^{2}{\left (t \right )} + y^{2}{\left (t \right )}\right)\right)\right)}\end{matrix}\right]$$



## Create a PyDy system


```python
constants = {
    m:1,
    Izz:1,
}

initial_conditions = {
    x:0,
    y:0,
    psi:0,
    u:0,
    v:0,
    r:0,
    
}

times = np.linspace(0,10,100)

sys = System(kane,constants=constants,initial_conditions=initial_conditions,times = times)
```


```python
result = sys.integrate()
```


```python
fig,ax = plt.subplots()
ax.plot(times,result[:,0])
ax.set_xlabel('time [s]')
ax.set_ylabel('x [m]')
```


```python
p = O.pos_from(O0).express(earth_frame)
x0_ = p.dot(ship_frame.x)
x0_
```




$$\left(x{\left (t \right )} \sin{\left (\psi{\left (t \right )} \right )} + y{\left (t \right )} \cos{\left (\psi{\left (t \right )} \right )}\right) \sin{\left (\psi{\left (t \right )} \right )} + \left(x{\left (t \right )} \cos{\left (\psi{\left (t \right )} \right )} - y{\left (t \right )} \sin{\left (\psi{\left (t \right )} \right )}\right) \cos{\left (\psi{\left (t \right )} \right )}$$




```python
y0_ = p.dot(ship_frame.y)
y0_
```




$$\left(x{\left (t \right )} \sin{\left (\psi{\left (t \right )} \right )} + y{\left (t \right )} \cos{\left (\psi{\left (t \right )} \right )}\right) \cos{\left (\psi{\left (t \right )} \right )} - \left(x{\left (t \right )} \cos{\left (\psi{\left (t \right )} \right )} - y{\left (t \right )} \sin{\left (\psi{\left (t \right )} \right )}\right) \sin{\left (\psi{\left (t \right )} \right )}$$




```python
f_x0 = sym.lambdify((x,y,psi,t),x0_,"numpy")
f_y0 = sym.lambdify((x,y,psi,t),y0_,"numpy")

```


```python
x0_sim = f_x0(result[:,0],result[:,1],result[:,2],times)
```


```python
fig,ax = plt.subplots()
ax.plot(times,x0_sim)
ax.set_xlabel('time [s]')
ax.set_ylabel('x0 [m]')
```


```python
y0_sim = f_y0(result[:,0],result[:,1],result[:,2],times)
```


```python
fig,ax = plt.subplots()
ax.plot(times,y0_sim)
ax.set_xlabel('time [s]')
ax.set_ylabel('y0 [m]')
```


```python
fig,ax = plt.subplots()
ax.plot(y0_sim,x0_sim)
ax.set_xlabel('y0 [m]')
ax.set_ylabel('x0 [m]')

```


```python

```

1d1f7e19e465b63376f517be63f3b0d56e229a34
ipynb
Jupyter Notebook
notebooks/10_experiments/3DOF.ipynb
martinlarsalbert/ForceMan
b07541f1c04d0ed103d7a5f66256812ebca2824c
MIT
notebooks/10_experiments/3DOF.ipynb
martinlarsalbert/ForceMan
b07541f1c04d0ed103d7a5f66256812ebca2824c
MIT
2019-12-15T17:23:21.000Z
2019-12-15T17:23:21.000Z
notebooks/10_experiments/3DOF.ipynb
martinlarsalbert/ForceMan
b07541f1c04d0ed103d7a5f66256812ebca2824c
MIT
Qwen/Qwen-72B
1. YES
2. YES