"""
This file will use pydy.codegen to simulate the double pendulum.
"""
from numpy import concatenate, array, linspace
from pydy.system import System
from scipy.integrate import odeint
from double_pendulum import *
constants = {l: 10.0, m: 10.0, g: 9.81}
initial_conditions = {q1: 1.0, q2: 0.0, u1: 0.0, u2: 0.0}
sys = System(KM, constants=constants,
initial_conditions=initial_conditions)
frames_per_sec = 60
final_time = 5.0
times = linspace(0.0, final_time, final_time * frames_per_sec)
sys.times = times
x = sys.integrate()
import sympy as _sm
import math as m
import numpy as _np
m, k, b, g = _sm.symbols('m k b g', real=True)
position, speed = _me.dynamicsymbols('position speed')
position_d, speed_d = _me.dynamicsymbols('position_ speed_', 1)
o = _me.dynamicsymbols('o')
force = o*_sm.sin(_me.dynamicsymbols._t)
frame_ceiling = _me.ReferenceFrame('ceiling')
point_origin = _me.Point('origin')
point_origin.set_vel(frame_ceiling, 0)
particle_block = _me.Particle('block', _me.Point('block_pt'), _sm.Symbol('m'))
particle_block.point.set_pos(point_origin, position*frame_ceiling.x)
particle_block.mass = m
particle_block.point.set_vel(frame_ceiling, speed*frame_ceiling.x)
force_magnitude = m*g-k*position-b*speed+force
force_block = (force_magnitude*frame_ceiling.x).subs({position_d:speed})
kd_eqs = [position_d - speed]
forceList = [(particle_block.point,(force_magnitude*frame_ceiling.x).subs({position_d:speed}))]
kane = _me.KanesMethod(frame_ceiling, q_ind=[position], u_ind=[speed], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([particle_block], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {m:1.0, k:1.0, b:0.2, g:9.8},
specifieds={_me.dynamicsymbols('t'):lambda x, t: t, o:2},
initial_conditions={position:0.1, speed:-1*1.0},
times = _np.linspace(0.0, 10.0, 10.0/0.01))
y=sys.integrate()
body_b_f.set_ang_vel(body_a_f, alpha*body_a_f.z)
point_o.set_vel(frame_n, 0)
body_a_cm.v2pt_theory(point_o,frame_n,body_a_f)
body_b_cm.v2pt_theory(point_o,frame_n,body_a_f)
ma = sm.symbols('ma')
body_a.mass = ma
mb = sm.symbols('mb')
body_b.mass = mb
iaxx = 1/12*ma*(2*la)**2
iayy = iaxx
iazz = 0
ibxx = 1/12*mb*h**2
ibyy = 1/12*mb*(w**2+h**2)
ibzz = 1/12*mb*w**2
body_a.inertia = (me.inertia(body_a_f, iaxx, iayy, iazz, 0, 0, 0), body_a_cm)
body_b.inertia = (me.inertia(body_b_f, ibxx, ibyy, ibzz, 0, 0, 0), body_b_cm)
force_a = body_a.mass*(g*frame_n.z)
force_b = body_b.mass*(g*frame_n.z)
kd_eqs = [thetad - omega, phid - alpha]
forceList = [(body_a.masscenter,body_a.mass*(g*frame_n.z)), (body_b.masscenter,body_b.mass*(g*frame_n.z))]
kane = me.KanesMethod(frame_n, q_ind=[theta,phi], u_ind=[omega, alpha], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([body_a, body_b], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {g:9.81, lb:0.2, w:0.2, h:0.1, ma:0.01, mb:0.1},
specifieds={},
initial_conditions={theta:np.deg2rad(90), phi:np.deg2rad(0.5), omega:0, alpha:0},
times = np.linspace(0.0, 10, 10/0.02))
y=sys.integrate()
coordinates = [x, y, beta, e]
speeds = [x_dot, y_dot, beta_dot, e_dot]
kane = me.KanesMethod(N, coordinates, speeds, kde)
loads = [
spring_1_force_P1, spring_2_force_P2, grav_force_plate, grav_force_rod,
spring_force_rod, spring_force_rod_on_plate, damping_rod,
damping_rod_on_plate, damping_1, damping_2, moment
]
fr, frstar = kane.kanes_equations(loads, [Plate, rod])
Mass = kane.mass_matrix_full
f = kane.forcing_full
sys = System(kane)
sys.constants = {
m: mass_of_rod,
M: mass_of_plate,
g: 9.81,
H: workspace_width,
a: plate_width,
b: plate_height,
c: cable_c,
c_rod: rod_c,
k_rod: rod_k,
Izz: inertia_of_plate,
Izz_rod: inertia_of_rod,
}
sys.initial_conditions = {x: x_init, y: y_init, beta: beta_init, e: -rod_init}
sys.specifieds = {
eqs[0].rhs, eqs[1].rhs, eqs[2].rhs, eqs[3].rhs, eqs[4].rhs, eqs[5].rhs,
eqs[6].rhs, eqs[7].rhs, eqs[8].rhs, eqs[9].rhs, eqs[10].rhs, eqs[11].rhs
]
kdes = [
q[0].diff() - u[0], q[1].diff() - u[1], q[2].diff() - u[2],
q[3].diff() - u[3], q[4].diff() - u[4], q[5].diff() - u[5],
q[6].diff() - u[6], q[7].diff() - u[7], q[8].diff() - u[8],
q[9].diff() - u[9], q[10].diff() - u[10], q[11].diff() - u[11]
]
kane = me.KanesMethod(N, q, u, kd_eqs=kdes)
Fr, Frst = kane.kanes_equations(bodies, loads=loads)
sys = System(kane)
sys.constants = {
I[0]: 1.0,
I[1]: 1.0,
I[2]: 1.0,
I[3]: 1.0,
I[4]: 1.0,
I[5]: 1.0,
L[0]: 0.3,
L[1]: 0.0,
L[2]: 0.5,
L[3]: 0.3,
L[4]: 0.0,
L[5]: 0.5,
L[6]: 0.3,
get_blade_thrust = [partial(_get_blade_thrust, i) for i in range(len(blade_frame))]
get_blade_torque = [partial(_get_blade_torque, i) for i in range(len(blade_frame))]
def v_1(x,t):
#ret = np.piecewise(t, [t <= 0.5, t > 0.5], [1, 0])
#return ret
return 1
def v_2(x,t):
return 1
def v_3(x,t):
return 1
def v_4(x,t):
return 1
bb_sys = System(KM)
bb_sys.initial_conditions = {
quat[0]: 0.,
quat[1]: 0.,
quat[2]: 0.,
quat[3]: 1.,
pos[0]: 0.,
pos[1]: 0.,
pos[2]: 0.,
motor_theta[0]: 0.,
motor_theta[1]: 0.,
motor_theta[2]: 0.,
motor_theta[3]: 0.,
blade_theta[0]: 0.0,
blade_theta[1]: 0.0,
import sympy as sm
import math as m
import numpy as np
m, k, b, g = sm.symbols('m k b g', real=True)
position, speed = me.dynamicsymbols('position speed')
positiond, speedd = me.dynamicsymbols('position speed', 1)
o = me.dynamicsymbols('o')
force = o*sm.sin(me.dynamicsymbols._t)
frame_ceiling = me.ReferenceFrame('ceiling')
point_origin = me.Point('origin')
point_origin.set_vel(frame_ceiling, 0)
particle_block = me.Particle('block', me.Point('block_pt'), sm.Symbol('m'))
particle_block.point.set_pos(point_origin, position*frame_ceiling.x)
particle_block.mass = m
particle_block.point.set_vel(frame_ceiling, speed*frame_ceiling.x)
force_magnitude = m*g-k*position-b*speed+force
force_block = (force_magnitude*frame_ceiling.x).subs({positiond:speed})
kd_eqs = [positiond - speed]
forceList = [(particle_block.point,(force_magnitude*frame_ceiling.x).subs({positiond:speed}))]
kane = me.KanesMethod(frame_ceiling, q_ind=[position], u_ind=[speed], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([particle_block], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {m:1.0, k:1.0, b:0.2, g:9.8},
specifieds={me.dynamicsymbols('t'):lambda x, t: t, o:2},
initial_conditions={position:0.1, speed:-1*1.0},
times = np.linspace(0.0, 10.0, 10.0/0.01))
y=sys.integrate()
def example_pydy(ax=None, **options):
"""
Example from the documentation of
:epkg:`pydy`.
@param ax matplotlib axis
@parm options extra options
@return ax
"""
# part 1
from sympy import symbols
import sympy.physics.mechanics as me
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
force = me.dynamicsymbols('F')
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
force_magnitude = mass * gravity - stiffness * position - damping * speed + force
forces = [(center, force_magnitude * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
# part 2
from numpy import linspace, sin
from pydy.system import System
sys = System(kane,
constants={
mass: 1.0,
stiffness: 1.0,
damping: 0.2,
gravity: 9.8
},
specifieds={
force: lambda x, t: sin(t)
},
initial_conditions={
position: 0.1,
speed: -1.0
},
times=linspace(0.0, 10.0, 1000))
y = sys.integrate()
# part 3
import matplotlib.pyplot as plt
if ax is None:
_, ax = plt.subplots(nrows=1,
ncols=1,
figsize=options.get('figsize', (5, 5)))
ax.plot(sys.times, y)
ax.legend((str(position), str(speed)))
return ax
# equations of motion with Kane's method
# make a tuple of the bodies and forces
bodies = (rod, plate)
loads = (rod_gravity, plate_gravity)
# create a Kane object with respect to the Newtonian reference frame
kane = me.KanesMethod(N,
q_ind=(theta, phi),
u_ind=(omega, alpha),
kd_eqs=kinDiffs)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.constants = {
lB: 0.2, # m
h: 0.1, # m
w: 0.2, # m
mA: 0.01, # kg
mB: 0.1, # kg
g: 9.81, # m/s**2
}
sys.initial_conditions = {
theta: np.deg2rad(90.0),
phi: np.deg2rad(0.5),
omega: 0,
alpha: 0
loads = [grav_force_plate, left_force, right_force]
fr, frstar = kane.kanes_equations(loads, [Plate])
Mass = kane.mass_matrix_full
f = kane.forcing_full
plate_width = 4.0
plate_height = 2.0
mass_of_plate = 10.0
rod_length = 3.0
mass_of_rod = 2.0
inertia_of_plate = (plate_width**2 + plate_height**2) * (mass_of_plate/12.0)
sys = System(kane)
sys.constants = {
M:12.0,
g:9.81,
F1:Lengths_and_Moments(inital_x, inital_y)[0],
F2:Lengths_and_Moments(inital_x, inital_y)[1],
H:20.0,
a:4.0,
b:2.0,
Izz:inertia_of_plate,
}
sys.initial_conditions = {x:inital_x, y:inital_y, beta:0}
sys.times = np.linspace(0.0, 20.0, 1000)
y = sys.integrate()
frame_a = _me.ReferenceFrame('a')
frame_b = _me.ReferenceFrame('b')
frame_a.orient(frame_n, 'Axis', [q1, frame_n.z])
frame_b.orient(frame_n, 'Axis', [q2, frame_n.z])
frame_a.set_ang_vel(frame_n, u1*frame_n.z)
frame_b.set_ang_vel(frame_n, u2*frame_n.z)
point_o = _me.Point('o')
particle_p = _me.Particle('p', _me.Point('p_pt'), _sm.Symbol('m'))
particle_r = _me.Particle('r', _me.Point('r_pt'), _sm.Symbol('m'))
particle_p.point.set_pos(point_o, l*frame_a.x)
particle_r.point.set_pos(particle_p.point, l*frame_b.x)
point_o.set_vel(frame_n, 0)
particle_p.point.v2pt_theory(point_o,frame_n,frame_a)
particle_r.point.v2pt_theory(particle_p.point,frame_n,frame_b)
particle_p.mass = m
particle_r.mass = m
force_p = particle_p.mass*(g*frame_n.x)
force_r = particle_r.mass*(g*frame_n.x)
kd_eqs = [q1_d - u1, q2_d - u2]
forceList = [(particle_p.point,particle_p.mass*(g*frame_n.x)), (particle_r.point,particle_r.mass*(g*frame_n.x))]
kane = _me.KanesMethod(frame_n, q_ind=[q1,q2], u_ind=[u1, u2], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([particle_p, particle_r], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {l:1, m:1, g:9.81},
specifieds={},
initial_conditions={q1:.1, q2:.2, u1:0, u2:0},
times = _np.linspace(0.0, 10, 10/.01))
y=sys.integrate()
coordinates = [x, y, beta, e]
speeds = [x_dot, y_dot, beta_dot, e_dot]
kane = me.KanesMethod(N, coordinates, speeds, kde)
loads = [
spring_1_force_P1, spring_2_force_P2, grav_force_plate, grav_force_rod,
spring_force_rod, spring_force_rod_on_plate, damping_rod,
damping_rod_on_plate, damping_1, damping_2, moment
]
fr, frstar = kane.kanes_equations(loads, [Plate, rod])
Mass = kane.mass_matrix_full
f = kane.forcing_full
sys = System(kane)
sys.constants = {
m: mass_of_rod,
M: mass_of_plate,
g: 9.81,
H: workspace_width,
a: plate_width,
b: plate_height,
c: cable_c,
c_rod: rod_c,
k_rod: rod_k,
Izz: inertia_of_plate,
Izz_rod: inertia_of_rod,
}
sys.initial_conditions = {x: x_init, y: y_init, beta: beta_init, e: -rod_init}
sys.times = np.linspace(0.0, Simulation_Time, 1000)
l23[0]: 0.01,
l23[1]: 0.01,
l23[2]: 0.01,
l34[0]: 0.01,
l34[1]: 0.01,
l34[2]: 0.01,
l45: 0.01,
l56: 0.02,
m1: 0.01,
m2: 0.05,
m3: 0.1,
m4: 0.01,
m5: 0.01 #, m6: 0.02
}
sys = System(KM)
sys.constants = consts
sys.times = np.linspace(0, 100.0, 1000000)
yi1 = consts[l01[1]] - consts[l10[1]]
yi2 = yi1 + consts[l12[1]] - consts[l21[1]]
x6 = consts[l45] + consts[l56]
sys.initial_conditions = {
q[0]: 0.0,
q[1]: 0.0,
q[2]: 0.0,
q[3]: 0.0,
q[4]: 0.0,
q[5]: 0.0,
q[6]: 0.0,
q[7]: yi1,
q[8]: 0.0,
C_frame_spring,
C_frame_spring_on_B
]
# In[7]:
# Setting up the coordinates, speeds, and creating KanesMethod
coordinates = [x, z, e, theta, phi]
speeds = [x_dot, z_dot, e_dot, theta_dot, phi_dot]
kane = me.KanesMethod(A, coordinates, speeds, kde)
# Creating Fr and Fr_star
fr, frstar = kane.kanes_equations(loads, [Plate, Rod])
# Creating the PyDy System
sys = System(kane)
# In[8]:
# Assign the constants
workspace_width = 10.4
runtime = 30
plate_width = 0.5
plate_height = 0.3
rod_radius = 0.02
rod_length = 2.91
plate_inertia = (plate_width**2 + plate_height**2) * (plate_mass / 12.0)
rod_inertia = (rod_mass / 12) * (3 * rod_radius**2 + rod_length**2)
plate_gravity = (Bo, mB * g * N.z)
# equations of motion with Kane's method
# make a tuple of the bodies and forces
bodies = (rod, plate)
loads = (rod_gravity, plate_gravity)
# create a Kane object with respect to the Newtonian reference frame
kane = me.KanesMethod(N, q_ind=(theta, phi), u_ind=(omega, alpha),
kd_eqs=kinDiffs)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.constants = {lB: 0.2, # m
h: 0.1, # m
w: 0.2, # m
mA: 0.01, # kg
mB: 0.1, # kg
g: 9.81, # m/s**2
}
sys.initial_conditions = {theta: np.deg2rad(90.0),
phi: np.deg2rad(0.5),
omega: 0,
alpha: 0}
sys.times = np.linspace(0, 10, 500)
def example_pydy(ax=None, **options):
"""
Example from the documentation of
:epkg:`pydy`.
@param ax matplotlib axis
@parm options extra options
@return ax
"""
# part 1
from sympy import symbols
import sympy.physics.mechanics as me
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
force = me.dynamicsymbols('F')
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
force_magnitude = mass * gravity - stiffness * position - damping * speed + force
forces = [(center, force_magnitude * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling, q_ind=[position], u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
# part 2
from numpy import linspace, sin
from pydy.system import System
sys = System(kane,
constants={mass: 1.0, stiffness: 1.0,
damping: 0.2, gravity: 9.8},
specifieds={force: lambda x, t: sin(t)},
initial_conditions={position: 0.1, speed: -1.0},
times=linspace(0.0, 10.0, 1000))
y = sys.integrate()
# part 3
import matplotlib.pyplot as plt
if ax is None:
_, ax = plt.subplots(
nrows=1, ncols=1, figsize=options.get('figsize', (5, 5)))
ax.plot(sys.times, y)
ax.legend((str(position), str(speed)))
return ax
