<jupyter_start><jupyter_text>Rigid body 3 DOFDevlop a system for a rigid body in 3 DOF and do a simualtion<jupyter_code>import warnings
#warnings.filterwarnings('ignore')
%matplotlib inline
%load_ext autoreload
%autoreload 2
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

from pydy.codegen.ode_function_generators import generate_ode_function
from scipy.integrate import odeint
from sympy import cos,sin

x0,y0,z0 = me.dynamicsymbols('x0 y0 z0')
x01d,y01d,z01d = me.dynamicsymbols('x01d y01d z01d')
u,v,w = me.dynamicsymbols('u v w')
u1d,v1d,w1d = me.dynamicsymbols('u v w',1)


phi,theta,psi = me.dynamicsymbols('phi theta psi')
phi1d,theta1d,psi1d = me.dynamicsymbols('phi1d theta1d psi1d')

t = sp.symbols('t')
N = me.ReferenceFrame('N')
S = N.orientnew('S', 'Axis', [psi,N.z])
S.ang_vel_in(N)
S.ang_acc_in(N)
M = me.Point('M')
O = M.locatenew('P',0)

M.set_vel(N,0)
O.set_vel(S,u*S.x + v*S.y)

O.v1pt_theory(M,N,S)
velocity_matrix = O.vel(N).to_matrix(N)
velocity_matrix<jupyter_output><empty_output><jupyter_text>Mass<jupyter_code>mass = sp.symbols('m')<jupyter_output><empty_output><jupyter_text>Inertia<jupyter_code>I_xx, I_yy, I_zz = sp.symbols('I_xx, I_yy, I_zz')
body_inertia_dyadic = me.inertia(S, ixx=I_xx, iyy=I_yy, izz=I_zz)
body_inertia_dyadic
body_inertia_dyadic.to_matrix(S)
body_central_inertia = (body_inertia_dyadic, O)
body = me.RigidBody('Rigid body', masscenter=O, frame = S,
                      mass=mass, inertia=body_central_inertia)<jupyter_output><empty_output><jupyter_text>Forces<jupyter_code>fx, fy, fz, mx, my, mz = sp.symbols('f_x f_y f_z m_x m_y m_z')
force_vector = fx*S.x + fy*S.y 
torque_vector = mz*S.z
force = (O, force_vector)
torque = (S, torque_vector)<jupyter_output><empty_output><jupyter_text>Equations of Motion<jupyter_code>coordinates = [x0, y0, psi]
speeds = [u, v, psi1d]
kinematical_differential_equations = [x0.diff() - velocity_matrix[0],                                      
                                      y0.diff() - velocity_matrix[1],
                                      psi.diff() - psi1d,
                                     ]
kinematical_differential_equations
#?me.KanesMethod
kane = me.KanesMethod(N, coordinates, speeds, kinematical_differential_equations)
loads = [force,
         torque]
bodies = [body]
fr, frstar = kane.kanes_equations(bodies, loads)
constants = [I_xx, I_yy, I_zz,mass]

specified = [fx, fy, mz]  # External force/torque

right_hand_side = generate_ode_function(kane.forcing_full, coordinates,
                                        speeds, constants,
                                        mass_matrix=kane.mass_matrix_full,specifieds=specified)
coordinates_ = [0, 0, 0,]
speeds_ = [0, 0, 0,]

start = np.array(coordinates_+speeds_)

t_ = 0.
force_torque = [1,0,0]
numerical_specified = np.array(force_torque)

I_xx_ = 1
I_yy_ = 1
I_zz_ = 1
mass_ = 1

numerical_constants = np.array([I_xx_, I_yy_, I_zz_,mass_])

right_hand_side(start, t_, numerical_specified, numerical_constants)
def simulate(t,force_torque, I_xx,I_yy,I_zz,mass, initial_coordinates = [0, 0, 0,], 
             initial_speeds = [0, 0, 0,]):
    
    start = np.array(initial_coordinates+initial_speeds)
    
    numerical_specified = force_torque
        
    numerical_constants = np.array([I_xx, I_yy, I_zz, mass])
    
    df = pd.DataFrame(index=t)
    y = odeint(right_hand_side, start, t, args=(numerical_specified, numerical_constants))
    
    for i,symbol in enumerate(coordinates+speeds):
        name = symbol.name
        df[name] = y[:,i]
        
    return df
t = np.linspace(0,10,100)

df = simulate(t=t, force_torque=[1,0,0],I_xx=1, I_yy=1, I_zz=1, mass=1)


fig,ax = plt.subplots()
df.plot(y='x0', ax=ax);
ax.set_xlabel('time [s]')

fig,ax = plt.subplots()
df.plot(y='u', ax=ax);
ax.set_xlabel('time [s]');
def track_plot(df,ax, l=1, time_step='1S'):
    
    df.plot(x='y0', y='x0',ax = ax)
    
    df_ = df.copy()
    df_.index = pd.TimedeltaIndex(df_.index,unit='s')
    df_ = df_.resample(time_step).first()   
    
    def plot_body(row):
        x = row['y0']
        y = row['x0']
        psi = row['psi']
        xs = [x-l/2*np.sin(psi),x+l/2*np.sin(psi)]
        ys = [y-l/2*np.cos(psi),y+l/2*np.cos(psi)]
        ax.plot(xs,ys,'k-')
    
    for index,row in df_.iterrows():
        plot_body(row)
    
    ax.set_xlabel('y0')
    ax.set_ylabel('x0')
    ax.axis('equal')
    
    
        
    
    
t = np.linspace(0,10,100)

df = simulate(t=t, force_torque=[0,0,0],I_xx=1, I_yy=1, I_zz=1, mass=1,
              initial_speeds=[1,0,0,],initial_coordinates=[0, 0, 0,])


fig,ax = plt.subplots()
track_plot(df,ax)


fig,ax = plt.subplots()
df.plot(y='psi', ax=ax);
ax.set_xlabel('time')
ax.set_ylabel('psi')



radius = 10  # Radius of rotation [m]
w = 0.1  # Angle velocity [rad/s]
V = radius*w  # Speed of point [m/s]
t = np.linspace(0,2*np.pi/w,100)

mass=1

expected_acceleration = -radius*w**2
expected_force = mass*expected_acceleration


df = simulate(t=t, force_torque=[0,-expected_force,0],I_xx=1, I_yy=1, I_zz=1, mass=mass,
              initial_speeds=[V,0,w,],initial_coordinates=[0, 0, 0,])


fig,ax = plt.subplots()
track_plot(df,ax, time_step = '5S')


fig,ax = plt.subplots()
df.plot(y='psi', ax=ax);
ax.set_xlabel('time')
ax.set_ylabel('psi')<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1473531016918319, 'fraction_numerical': 0.023285428415499364, 'mean_word_length': 3.7274290627687017, 'pattern_counts': {'":': 0, '<': 21, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 7, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '5', 'dir': 'jupyter-structured-clean-dedup', 'id': '665691', 'license': 'MIT', 'n_tokens_mistral': 2373, 'n_tokens_neox': 2269, 'n_words': 392, 'path': 'rigid_body_3DOF.ipynb', 'repo_name': 'martinlarsalbert/motion-to-acceleration', 'size': '5497'}