# Pendulum with Python package PyDy


```python
from __future__ import print_function, division
from sympy import symbols, simplify
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point
from sympy.physics.vector import init_vprinting
init_vprinting(use_latex='mathjax', pretty_print=False)
from sympy.physics.mechanics import inertia, RigidBody
from sympy import trigsimp
from sympy.physics.mechanics import KanesMethod
from numpy import deg2rad, rad2deg, array, zeros, linspace
from scipy.integrate import odeint
from pydy.codegen.ode_function_generators import generate_ode_function
import matplotlib.pyplot as plt
import ipywidgets as widgets
from ivisual import *
from IPython.display import display, clear_output
```


<div id="scene0"><div id="glowscript" class="glowscript"></div></div>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>



    <IPython.core.display.Javascript object>


## Reference frames


```python
inertial_frame = ReferenceFrame('I')
pendulum_frame = ReferenceFrame('P')
```

    (MainThread) backend module://ipykernel.pylab.backend_inline version unknown


    ERROR! Session/line number was not unique in database. History logging moved to new session 440



```python
psi = dynamicsymbols('psi') #Pendulum angle
```


```python
pendulum_frame.orient(inertial_frame, 'Axis', (psi, inertial_frame.z))
```


```python
pendulum_frame.dcm(inertial_frame)
```




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



## Define some points


```python
attachment = Point('A')
distance_attachment_pendulum_mass_center = symbols('Lcg')

pendulum_mass_center = Point('cg')
pendulum_mass_center.set_pos(attachment, distance_attachment_pendulum_mass_center*pendulum_frame.y)

pendulum_length = symbols('L')
pendulum_endpoint = Point('end')
pendulum_endpoint.set_pos(attachment, pendulum_length*pendulum_frame.y)

```


```python
position = pendulum_endpoint.pos_from(attachment)
position.to_matrix(inertial_frame)
```




$$\left[\begin{matrix}- L \operatorname{sin}\left(\psi\right)\\L \operatorname{cos}\left(\psi\right)\\0\end{matrix}\right]$$



## Kinematical Differential Equations


```python
r = dynamicsymbols('r') # Pendulum rate
```


```python
kinematical_differential_equations = [r - psi.diff()]
kinematical_differential_equations
```




$$\left [ r - \dot{\psi}\right ]$$



## Angular Velocities


```python
pendulum_frame.set_ang_vel(inertial_frame,r*inertial_frame.z)
```


```python
pendulum_frame.ang_vel_in(inertial_frame)
```




$$r\mathbf{\hat{i}_z}$$



## Linear Velocities


```python
attachment.set_vel(inertial_frame, 0)

```


```python
pendulum_mass_center.v2pt_theory(attachment, inertial_frame, pendulum_frame)
```




$$-  Lcg r\mathbf{\hat{p}_x}$$



## Mass


```python
pendulum_mass = symbols('m_P')
```

## Inertia


```python
Izz = symbols('I_zz')
```


```python
pendulum_dyadic = inertia(pendulum_frame, 0, 0, Izz)
```


```python
pendulum_dyadic
```




$$I_{zz}\mathbf{\hat{p}_z}\otimes \mathbf{\hat{p}_z}$$




```python
pendulum_dyadic.to_matrix(pendulum_frame)
```




$$\left[\begin{matrix}0 & 0 & 0\\0 & 0 & 0\\0 & 0 & I_{zz}\end{matrix}\right]$$




```python
pendulum_central_inertia = (pendulum_dyadic, pendulum_mass_center)
```


```python
pendulum = RigidBody('Pendulum', pendulum_mass_center, pendulum_frame,
                      pendulum_mass, pendulum_central_inertia)
```

## Equations of Motion


```python
coordinates = [psi]
coordinates
```




$$\left [ \psi\right ]$$




```python
speeds = [r]
speeds
```




$$\left [ r\right ]$$




```python
kane = KanesMethod(inertial_frame, coordinates, speeds, kinematical_differential_equations)
```


```python
g = symbols('g')
```


```python
pendulum_grav_force_vector = pendulum_mass * g * inertial_frame.y
pendulum_grav_force_vector
```




$$g m_{P}\mathbf{\hat{i}_y}$$




```python
pendulum_grav_force = (pendulum_mass_center, pendulum_grav_force_vector)
```


```python
attachement_torque = dynamicsymbols('T_a')
pendulum_torque_vector = attachement_torque * inertial_frame.z
pendulum_torque_vector
```




$$T_{a}\mathbf{\hat{i}_z}$$




```python
pendulum_torque = (pendulum_frame, pendulum_torque_vector)
```


```python
#loads = [pendulum_grav_force,
#        pendulum_torque]
loads = [pendulum_grav_force]

loads
```




    [(cg, g*m_P*I.y)]




```python
bodies = [pendulum]
```


```python
fr, frstar = kane.kanes_equations(bodies = bodies,loads = loads)
```


```python
trigsimp(fr + frstar)
```




$$\left[\begin{matrix}- Lcg g m_{P} \operatorname{sin}\left(\psi\right) - \left(I_{zz} + Lcg^{2} m_{P}\right) \dot{r}\end{matrix}\right]$$




```python
mass_matrix = trigsimp(kane.mass_matrix_full)
mass_matrix
```




$$\left[\begin{matrix}1 & 0\\0 & I_{zz} + Lcg^{2} m_{P}\end{matrix}\right]$$




```python
forcing_vector = trigsimp(kane.forcing_full)
forcing_vector
```




$$\left[\begin{matrix}r\\- Lcg g m_{P} \operatorname{sin}\left(\psi\right)\end{matrix}\right]$$



## Simulation


```python
constants = [distance_attachment_pendulum_mass_center,
             pendulum_mass,            
             Izz,
             g]
```


```python
coordinates = [psi]
coordinates
```




$$\left [ \psi\right ]$$




```python
speeds = [r]
speeds
```




$$\left [ r\right ]$$




```python
#specified = [pendulum_torque]
specified = []


right_hand_side = generate_ode_function(forcing_vector, coordinates,
                                        speeds, constants,
                                        mass_matrix=mass_matrix,
                                        specifieds=specified)
```


```python
help(right_hand_side)
```

    Help on function rhs in module pydy.codegen.ode_function_generators:
    
    rhs(*args)
        Returns the derivatives of the states, i.e. numerically evaluates the right
        hand side of the first order differential equation.
        
        x' = f(x, t, r, p)
        
        Parameters
        ==========
        x : ndarray, shape(2,)
            The state vector is ordered as such:
                - psi(t)
                - r(t)
        t : float
            The current time.
        r : dictionary; ndarray, shape(0,); function
        
            There are three options for this argument. (1) is more flexible but
            (2) and (3) are much more efficient.
        
            (1) A dictionary that maps the specified functions of time to floats,
            ndarrays, or functions that produce ndarrays. The keys can be a single
            specified symbolic function of time or a tuple of symbols. The total
            number of symbols must be equal to 0. If the value is a
            function it must be of the form g(x, t), where x is the current state
            vector ndarray and t is the current time float and it must return an
            ndarray of the correct shape. For example::
        
              r = {a: 1.0,
                   (d, b) : np.array([1.0, 2.0]),
                   (e, f) : lambda x, t: np.array(x[0], x[1]),
                   c: lambda x, t: np.array(x[2])}
        
            (2) A ndarray with the specified values in the correct order and of the
            correct shape.
        
            (3) A function that must be of the form g(x, t), where x is the current
            state vector and t is the current time and it must return an ndarray of
            the correct shape.
        
            The specified inputs are, in order:
                - 
        p : dictionary len(4) or ndarray shape(4,)
            Either a dictionary that maps the constants symbols to their numerical
            values or an array with the constants in the following order:
                - Lcg
                - m_P
                - I_zz
                - g
        
        Returns
        =======
        dx : ndarray, shape(2,)
            The derivative of the state vector.
    


## Set the Initial Conditions, Parameter Values, and Time Array


```python
x0 = array([deg2rad(50),0])
x0
```




    array([0.87266463, 0.        ])




```python
L = 5
numerical_constants = array([
                             L,  # pendulum_length [m]
                             10,  # pendulum_mass [kg]
                             10,  # pendulum_inertia [kg*m^2]
                             9.81],  # acceleration due to gravity [m/s^2]
                            ) 




```


```python
# %load exercise_solutions/n07_simulation_sim-setup.py
numerical_specified = zeros(3)

args = {'constants': numerical_constants,
        'specified': numerical_specified}

frames_per_sec = 60
final_time = 5.0

t = linspace(0.0, final_time, final_time * frames_per_sec)
```

    /Users/martinalexandersson/Dev/ForceMan/venv/lib/python3.6/site-packages/ipykernel_launcher.py:10: DeprecationWarning: object of type <class 'float'> cannot be safely interpreted as an integer.
      # Remove the CWD from sys.path while we load stuff.



```python
numerical_specified = array([0.0])

args = {'constants': numerical_constants,
        'specified': numerical_specified}

frames_per_sec = 60
final_time = 10.0

t = linspace(0.0, final_time, final_time * frames_per_sec)
```

    /Users/martinalexandersson/Dev/ForceMan/venv/lib/python3.6/site-packages/ipykernel_launcher.py:9: DeprecationWarning: object of type <class 'float'> cannot be safely interpreted as an integer.
      if __name__ == '__main__':



```python
right_hand_side(x0, 0.0, numerical_specified, numerical_constants)
```




    array([ 0.        , -1.44517231])



## Integrate the Equations of Motion


```python
y = odeint(right_hand_side, x0, t, args=(numerical_specified, numerical_constants))
```


```python
fig,ax = plt.subplots()
fig.set_size_inches(15,5)

ax.plot(t,y[:,0])
```


```python
fig,ax = plt.subplots()
fig.set_size_inches(15,5)

ax.plot(t,y[:,1])
```

## Visualization


```python
position = pendulum_endpoint.pos_from(attachment)
global_position = position.to_matrix(inertial_frame)
```


```python
psi_now = 0.2
(L*np.sin(psi_now),L*np.cos(psi_now),0)
```




$$\left ( 0.9933466539753061, \quad 4.900332889206208, \quad 0\right )$$




```python
a = tuple(global_position.evalf(subs = {pendulum_length:L,psi:psi_now}))
```


```python
a = global_position.evalf(subs = {pendulum_length:L,psi:psi_now})
np.array(a.transpose().tolist()).astype(np.float64)[0]
```




    array([-0.99334665,  4.90033289,  0.        ])




```python
pendulum_cylinder.
```


```python

def play(x):
    
    index = int((len(t)-1)*x/100)
    psi_now = y[index,0]
    
    a = global_position.evalf(subs = {pendulum_length:L,psi:psi_now})
    pendulum_cylinder.axis = np.array(a.transpose().tolist()).astype(np.float64)[0]
    

def play2(change):

    #x = interactive_plot.children[0].value
    x = slider.value

    play(x = x)

def plot(x):
    
    index = int((len(t)-1)*x/100)
    fig,ax = plt.subplots()
    ax.plot(t,y[:,0])
    ax.plot(t[index],y[index,0],'ro')
    display(fig)
    clear_output(wait=True)
    
    
```


```python

from ipywidgets import interactive


interactive_plot = interactive(plot, x=(0.0,100.0))
display(interactive_plot)

```


    interactive(children=(FloatSlider(value=50.0, description='x'), Output()), _dom_classes=('widget-interact',))



```python
scene2 = canvas(width = 1000,height = 500,up=vector(0,-1,0))
vscale = 3
varr = arrow(pos=vector(0,0,0), axis=vscale*vector(1,0,0), color=color.red,shaftwidth=vscale*0.1)
varr = arrow(pos=vector(0,0,0), axis=vscale*vector(0,1,0), color=color.green,shaftwidth=vscale*0.1)
varr = arrow(pos=vector(0,0,0), axis=vscale*vector(0,0,1), color=color.orange,shaftwidth=vscale*0.1)

pendulum_radius = 0.1
pendulum_cylinder = cylinder(pos = (0,0,0),axis = (0,1,0),length = L,radius = pendulum_radius)

play_widget = widgets.Play(
    interval=0.5,
    value=0,
    min=0,
    max=100,
    description="Press play",
    disabled=False

)
slider = widgets.FloatSlider()
widgets.jslink((play_widget, 'value'), (slider, 'value'))
#widgets.jslink((play_widget, 'value'), (interactive_plot.children[0], 'value'))


play_widget.observe(play2, names='value')

scene2.visible = True
display(scene2)
#display(interactive_plot.children[1])

widgets.HBox([play_widget, slider])
#widgets.HBox([play_widget, interactive_plot.children[0]])

```


<div id="scene0"><div id="glowscript" class="glowscript"></div></div>



    <IPython.core.display.Javascript object>



<div id="glowscript2" ><div id="glowscript" class="glowscript"></div></div>



    HBox(children=(Play(value=0, description='Press play', interval=0), FloatSlider(value=0.0)))



```python

```

63b8f9d9450e97e8d66ed54c3ae39881a1743799
ipynb
Jupyter Notebook
notebooks/01_dynamics_experiments/pendulum.ipynb
martinlarsalbert/ForceMan
b07541f1c04d0ed103d7a5f66256812ebca2824c
MIT
notebooks/01_dynamics_experiments/pendulum.ipynb
martinlarsalbert/ForceMan
b07541f1c04d0ed103d7a5f66256812ebca2824c
MIT
2019-12-15T17:23:21.000Z
2019-12-15T17:23:21.000Z
notebooks/01_dynamics_experiments/pendulum.ipynb
martinlarsalbert/ForceMan
b07541f1c04d0ed103d7a5f66256812ebca2824c
MIT
Qwen/Qwen-72B
1. YES
2. YES