def test_create_static_html():
# derive simple system
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
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]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
sys = System(kane,
initial_conditions={
position: 0.1,
speed: -1.0
},
constants={
mass: 1.0,
stiffness: 1.0,
damping: 0.2,
gravity: 9.8
})
# integrate eoms
t = linspace(0.0, 10.0, 100)
sys.times = t
y = sys.integrate()
# create visualization
sphere = Sphere()
viz_frame = VisualizationFrame(ceiling, block, sphere)
scene = Scene(ceiling, origin, viz_frame)
scene.generate_visualization_json_system(sys, outfile_prefix="test")
# test static dir creation
scene.create_static_html(overwrite=True)
assert os.path.exists('static')
assert os.path.exists('static/index.html')
assert os.path.exists('static/test_scene_desc.json')
assert os.path.exists('static/test_simulation_data.json')
# test static dir deletion
scene.remove_static_html(force=True)
assert not os.path.exists('static')
def test_create_static_html():
# derive simple system
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
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]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
constants = (mass, stiffness, damping, gravity)
coordinates = (position, )
speeds = (speed, )
system = (mass_matrix, forcing_vector, constants, coordinates, speeds)
# integrate eoms
evaluate_ode = generate_ode_function(*system)
x0 = array([0.1, -1.0])
args = {'constants': array([1.0, 1.0, 0.2, 9.8])}
t = linspace(0.0, 10.0, 100)
y = odeint(evaluate_ode, x0, t, args=(args, ))
# create visualization
sphere = Sphere()
viz_frame = VisualizationFrame(ceiling, block, sphere)
scene = Scene(ceiling, origin, viz_frame)
scene.generate_visualization_json([position, speed], list(constants), y,
args['constants'])
# test static dir creation
scene.create_static_html(overwrite=True)
assert os.path.exists('static')
assert os.path.exists('static/index.html')
assert os.path.exists('static/data.json')
# test static dir deletion
scene.remove_static_html(force=True)
assert not os.path.exists('static')
def setup(self):
"""Setups a simple 1 DoF mass spring damper system visualization."""
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
kinematic_equations = [speed - positiond]
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)
particles = [block]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
if sympy_newer_than('1.0'):
kane.kanes_equations(particles, forces)
else:
kane.kanes_equations(forces, particles)
self.sys = System(kane)
self.sys.initial_conditions = {position: 0.1, speed: -1.0}
self.sys.constants = {
mass: 1.0,
stiffness: 2.0,
damping: 3.0,
gravity: 9.8
}
self.sys.times = np.linspace(0.0, 0.01, 2)
sphere = Sphere()
self.ref_frame = ceiling
self.origin = origin
self.viz_frame = VisualizationFrame(ceiling, block, sphere)
self.viz_frame_sym_shape = VisualizationFrame(
ceiling, block, Sphere(radius=mass / 10.0))
def setup(self):
# This is taken from the example in KanesMethod docstring
# System state variables
q, u = me.dynamicsymbols('q u')
qd, ud = me.dynamicsymbols('q u', 1)
# Other system variables
m, c, k = symbols('m c k')
# Set up the reference frame
N = me.ReferenceFrame('N')
# Set up the point and particle
P = me.Point('P')
P.set_vel(N, u * N.x)
pa = me.Particle('pa', P, m)
# Create the list of kinematic differential equations, force list and
# list of bodies/particles
kd = [qd - u]
force_list = [(P, (-k * q - c * u) * N.x)]
body_list = [pa]
# Create an instance of KanesMethod
self.KM = me.KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
# Account for the new method of input to kanes_equations, i.e. the
# order of the args in the old API is forces, bodies and the new API
# is bodies, forces.
try:
self.KM.kanes_equations(body_list, force_list)
self.first_input = body_list
self.second_input = force_list
except TypeError:
self.first_input = force_list
self.second_input = body_list
frame_a = me.ReferenceFrame('a')
frame_b = me.ReferenceFrame('b')
frame_n = me.ReferenceFrame('n')
x1, x2, x3 = me.dynamicsymbols('x1 x2 x3')
l = sm.symbols('l', real=True)
v1 = x1 * frame_a.x + x2 * frame_a.y + x3 * frame_a.z
v2 = x1 * frame_b.x + x2 * frame_b.y + x3 * frame_b.z
v3 = x1 * frame_n.x + x2 * frame_n.y + x3 * frame_n.z
v = v1 + v2 + v3
point_c = me.Point('c')
point_d = me.Point('d')
point_po1 = me.Point('po1')
point_po2 = me.Point('po2')
point_po3 = me.Point('po3')
particle_l = me.Particle('l', me.Point('l_pt'), sm.Symbol('m'))
particle_p1 = me.Particle('p1', me.Point('p1_pt'), sm.Symbol('m'))
particle_p2 = me.Particle('p2', me.Point('p2_pt'), sm.Symbol('m'))
particle_p3 = me.Particle('p3', me.Point('p3_pt'), sm.Symbol('m'))
body_s_cm = me.Point('s_cm')
body_s_cm.set_vel(frame_n, 0)
body_s_f = me.ReferenceFrame('s_f')
body_s = me.RigidBody('s', body_s_cm, body_s_f, sm.symbols('m'),
(me.outer(body_s_f.x, body_s_f.x), body_s_cm))
body_r1_cm = me.Point('r1_cm')
body_r1_cm.set_vel(frame_n, 0)
body_r1_f = me.ReferenceFrame('r1_f')
body_r1 = me.RigidBody('r1', body_r1_cm, body_r1_f, sm.symbols('m'),
(me.outer(body_r1_f.x, body_r1_f.x), body_r1_cm))
body_r2_cm = me.Point('r2_cm')
body_r2_cm.set_vel(frame_n, 0)
frame_a = me.ReferenceFrame("a")
frame_b = me.ReferenceFrame("b")
frame_n = me.ReferenceFrame("n")
x1, x2, x3 = me.dynamicsymbols("x1 x2 x3")
l = sm.symbols("l", real=True)
v1 = x1 * frame_a.x + x2 * frame_a.y + x3 * frame_a.z
v2 = x1 * frame_b.x + x2 * frame_b.y + x3 * frame_b.z
v3 = x1 * frame_n.x + x2 * frame_n.y + x3 * frame_n.z
v = v1 + v2 + v3
point_c = me.Point("c")
point_d = me.Point("d")
point_po1 = me.Point("po1")
point_po2 = me.Point("po2")
point_po3 = me.Point("po3")
particle_l = me.Particle("l", me.Point("l_pt"), sm.Symbol("m"))
particle_p1 = me.Particle("p1", me.Point("p1_pt"), sm.Symbol("m"))
particle_p2 = me.Particle("p2", me.Point("p2_pt"), sm.Symbol("m"))
particle_p3 = me.Particle("p3", me.Point("p3_pt"), sm.Symbol("m"))
body_s_cm = me.Point("s_cm")
body_s_cm.set_vel(frame_n, 0)
body_s_f = me.ReferenceFrame("s_f")
body_s = me.RigidBody(
"s",
body_s_cm,
body_s_f,
sm.symbols("m"),
(me.outer(body_s_f.x, body_s_f.x), body_s_cm),
)
body_r1_cm = me.Point("r1_cm")
body_r1_cm.set_vel(frame_n, 0)
# Set dynamics variables
xc = mech.dynamicsymbols('xc'); # Cart position
xb = mech.dynamicsymbols('xb'); # Box position
xcDot = mech.dynamicsymbols('xcDot'); # Cart velocity
xbDot = mech.dynamicsymbols('xbDot'); # Box velocity
# Create symboloic variables we'll need
m, L, k, g, theta = sympy.symbols('m L k g theta');
# Create reference frame for analysis
N = mech.ReferenceFrame( 'N' );
# Set cart position, mass, and velocity
cPoint = mech.Point( 'cPoint' );
cPoint.set_vel(N, xcDot*N.x);
C = mech.Particle('C', cPoint, 4*m );
# Set box position, mass, and velocity
# Create rotated frame
boxFrame = mech.ReferenceFrame('boxFrame');
boxFrame.orient(N, 'Body', [0, 0, -theta], 'XYZ');
bPoint = mech.Point('bPoint');
bPoint.set_vel(N, xbDot*boxFrame.x);
B = mech.Particle('B', bPoint, m );
# Get the kinetic energy of each component to check results
mech.kinetic_energy(N, C)
mech.kinetic_energy(N, B)
# Set potential energies
C.potential_energy = (1/2)*(2*k)*xc**2;
def integrate_pendulum_odes(self, adv_time_vector=None, pos_init=235):
"""
Method to integrate Nth-order pendulum ODEs.
"""
# Instantiates physics constants for position, velocity, mass, length, gravity, and time
q, u = mechanics.dynamicsymbols("q:{0}".format(str(
self.N))), mechanics.dynamicsymbols("u:{0}".format(str(self.N)))
m, l = symbols("m:{0}".format(str(self.N))), symbols("l:{0}".format(
str(self.N)))
g, t = symbols("g t")
# Creates intertial reference frame
frame = mechanics.ReferenceFrame("frame")
# Creates focus point in intertial reference frame
point = mechanics.Point("point")
point.set_vel(frame, 0)
# Instantiates empty objects for pendulum segment points, reactionary forces, and resultant kinematic ODEs
particles, forces, kin_odes = list(), list(), list()
# Iteratively creates pendulum segment kinematics/dynamics objects
for iterator in range(self.N):
# Creates and sets angular velocity per reference frame for each pendulum segment
frame_i = frame.orientnew("frame_{}".format(str(iterator)), "Axis",
[q[iterator], frame.z])
frame_i.set_ang_vel(frame, u[iterator] * frame.z)
# Creates and sets velocity of focus point for each pendulum segment
point_i = point.locatenew("point_{}".format(str(iterator)),
l[iterator] * frame_i.x)
point_i.v2pt_theory(point, frame, frame_i)
# Creates reference point for each pendulum segment
ref_point_i = mechanics.Particle(
"ref_point_{}".format(str(iterator)), point_i, m[iterator])
particles.append(ref_point_i)
# Creates objects for reference frame dynamics
forces.append((point_i, m[iterator] * g * frame.x))
kin_odes.append(q[iterator].diff(t) - u[iterator])
point = point_i
# Generates position and velocity equation systems using Kane's Method
kane = mechanics.KanesMethod(frame, q_ind=q, u_ind=u, kd_eqs=kin_odes)
fr, frstar = kane.kanes_equations(particles, forces)
# Creates vector for initial positions and velocities
y0 = np.deg2rad(
np.concatenate([
np.broadcast_to(pos_init, self.N),
np.broadcast_to(self.vel_init, self.N)
]))
# Creates vectors for pendulum segment lengths and masses
length_vector = np.ones(self.N) / self.N
length_vector = np.broadcast_to(length_vector, self.N)
self.mass_vector = np.broadcast_to(self.mass_vector, self.N)
# Instantiates and creates fixed constant vectors (gravity, lengths, masses)
params = [g] + list(l) + list(m)
param_vals = [9.81] + list(length_vector) + list(self.mass_vector)
# Initializes objects to solve for unknown parameters
dummy_params = [Dummy() for iterator in q + u]
dummy_dict = dict(zip(q + u, dummy_params))
# Converts unknown parametric objects into Kane's Method objects for numerical substitution
kin_diff_dict = kane.kindiffdict()
mass_matrix = kane.mass_matrix_full.subs(kin_diff_dict).subs(
dummy_dict)
full_forcing_vector = kane.forcing_full.subs(kin_diff_dict).subs(
dummy_dict)
# Functionalizes Kane's Method unknown parametric objects using NumPy for numerical substitution
mm_func = lambdify(dummy_params + params, mass_matrix)
ff_func = lambdify(dummy_params + params, full_forcing_vector)
# Defines helper method with gradient calculus to use with ODE integration
def __parametric_gradient_function(y, t, args):
"""
Helper function to derive first-order equations of motion from parametric arguments.
"""
values = np.concatenate((y, args))
solutions = np.linalg.solve(mm_func(*values), ff_func(*values))
return np.array(solutions).T[0]
if adv_time_vector is None:
return odeint(__parametric_gradient_function,
y0,
self.time_vector,
args=(param_vals, ))
else:
return odeint(__parametric_gradient_function,
y0,
adv_time_vector,
args=(param_vals, ))
def pendulum_energy(n=1, lengths=1, masses=1, include_gpe=True, include_ke=True):
# Generalized coordinates and velocities
# (in this case, angular positions & velocities of each mass)
q = mechanics.dynamicsymbols('q:{0}'.format(n))
u = mechanics.dynamicsymbols('u:{0}'.format(n))
# mass and length
m = symbols('m:{0}'.format(n))
l = symbols('l:{0}'.format(n))
# gravity and time symbols
g, t = symbols('g,t')
#--------------------------------------------------
# Step 2: build the model using Kane's Method
# Create pivot point reference frame
A = mechanics.ReferenceFrame('A')
P = mechanics.Point('P')
Origin = P
P.set_vel(A, 0)
gravity_direction = -A.x
# lists to hold particles, forces, and kinetic ODEs
# for each pendulum in the chain
particles = []
forces = []
gpe = []
ke = []
cartVel = 0.0
cartPos = 0.0
for i in range(n):
# Create a reference frame following the i^th mass
Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z])
Ai.set_ang_vel(A, u[i] * A.z)
# Create a point in this reference frame
Pi = P.locatenew('P' + str(i), l[i] * Ai.x)
Pi.v2pt_theory(P, A, Ai)
# Create a new particle of mass m[i] at this point
Pai = mechanics.Particle('Pa' + str(i), Pi, m[i])
particles.append(Pai)
# Calculate the cartesian position and velocity:
# cartPos += l[i] * q[i]
pos = Pi.pos_from(Origin)
ke.append(1/n * Pai.kinetic_energy(A))
gpe.append(m[i] * g * (Pi.pos_from(Origin) & gravity_direction))
P = Pi
# lengths and masses
if lengths is None:
lengths = np.ones(n) / n
lengths = np.broadcast_to(lengths, n)
masses = np.broadcast_to(masses, n)
# Fixed parameters: gravitational constant, lengths, and masses
parameters = [g] + list(l) + list(m)
parameter_vals = [9.81] + list(lengths) + list(masses)
# define symbols for unknown parameters
unknowns = [Dummy() for i in q + u]
unknown_dict = dict(zip(q + u, unknowns))
# create functions for numerical calculation
total_energy = 0
if include_gpe:
total_energy += (sum(gpe)).subs(zip(parameters, parameter_vals))
if include_ke:
total_energy += (sum( ke)).subs(zip(parameters, parameter_vals))
total_energy_func = sympy2torch.sympy2torch(total_energy)
minimum_energy = total_energy_func(**fixvalue(n, torch.tensor([[0.]*2*n]))).detach()
return lambda inp: (total_energy_func(**fixvalue(n, inp)) - minimum_energy.to(inp)).unsqueeze(1)
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
def generate_n_link_pendulum_on_cart_equations_of_motion(n, cart_force=True):
"""Returns the implicit form of the symbolic equations of motion for a
2D n-link pendulum on a sliding cart under the influence of gravity.
Parameters
----------
n : integer
The number of links in the pendulum.
cart_force : boolean, default=True
If true an external specified lateral force is applied to the cart.
Returns
-------
mass_matrix : sympy.MutableMatrix, shape(2 * (n + 1), 2 * (n + 1))
The symbolic mass matrix of the system which are linear in u' and q'.
forcing_vector : sympy.MutableMatrix, shape(2 * (n + 1), 1)
The forcing vector of the system.
constants : list
A sequence of all the symbols which are constants in the equations
of motion.
coordinates : list
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the configuration of the system.
speeds : list
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the generalized speeds of the system.
specfied : list
Notes
-----
The degrees of freedom of the system are n + 1, i.e. one for each
pendulum link and one for the lateral motion of the cart.
M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
"""
q = me.dynamicsymbols('q:' + str(n + 1))
u = me.dynamicsymbols('u:' + str(n + 1))
m = symbols('m:' + str(n + 1))
l = symbols('l:' + str(n))
g, t = symbols('g t')
I = me.ReferenceFrame('I')
O = me.Point('O')
O.set_vel(I, 0)
P0 = me.Point('P0')
P0.set_pos(O, q[0] * I.x)
P0.set_vel(I, u[0] * I.x)
Pa0 = me.Particle('Pa0', P0, m[0])
frames = [I]
points = [P0]
particles = [Pa0]
forces = [(P0, -m[0] * g * I.y)]
kindiffs = [q[0].diff(t) - u[0]]
for i in range(n):
Bi = I.orientnew('B' + str(i), 'Axis', [q[i + 1], I.z])
Bi.set_ang_vel(I, u[i + 1] * I.z)
frames.append(Bi)
Pi = points[-1].locatenew('P' + str(i + 1), l[i] * Bi.x)
Pi.v2pt_theory(points[-1], I, Bi)
points.append(Pi)
Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
particles.append(Pai)
forces.append((Pi, -m[i + 1] * g * I.y))
kindiffs.append(q[i + 1].diff(t) - u[i + 1])
if cart_force is True:
F = me.dynamicsymbols('F')
forces.append((P0, F * I.x))
specified = [F]
else:
specified = None
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
kane.kanes_equations(forces, particles)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
coordinates = kane._q
speeds = kane._u
constants = [g, m[0]]
for i in range(n):
constants += [l[i], m[i + 1]]
return (mass_matrix, forcing_vector, constants, coordinates, speeds,
specified)
def generate_mass_spring_damper_equations_of_motion(external_force=True):
"""Returns the symbolic equations of motion and associated variables for a
simple one degree of freedom mass, spring, damper system with gravity
and an optional external specified force.
/ / / / / / / /
----------------
| | | | g
\ | | | V
k / --- c |
| | | x, v
-------- V
| m | -----
--------
| F
V
Returns
-------
mass_matrix : sympy.MutableMatrix, shape(2,2)
The symbolic mass matrix of the system which are linear in
derivatives of the states.
forcing_vector : sympy.MutableMatrix, shape(2,1)
The forcing vector of the system.
constants : tuple, len(4)
A sequence of all the symbols which are constants in the equations
of motion, i.e. m, k, c, g.
coordinates : tuple, len(1)
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the configuration of the system, i.e. x.
speeds : tuple, len(1)
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the generalized speeds of the system, i.e v.
specified : tuple, len(1) or None
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the specified variables of the system, i.e F. If
`external_force` is False, then None is returned.
external_force : boolean, default=True
If true a specifed force will be added to the system.
"""
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]
total_force = mass * gravity - stiffness * position - damping * speed
if external_force is True:
total_force += force
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
constants = (mass, stiffness, damping, gravity)
coordinates = (position, )
speeds = (speed, )
if external_force is True:
specified = (force, )
else:
specified = None
return (mass_matrix, forcing_vector, constants, coordinates, speeds,
specified)
SG.v2pt_theory(G, inertial_rf, girdle_rf)
points.append(SG)
S1 = me.Point('S1')
S1.set_pos(SG, -l_s*spine1_rf.x)
S1.v2pt_theory(SG, inertial_rf, spine1_rf)
points.append(S1)
#print(S1.vel(inertial_rf))
#### PARTICLES (Point masses are used instead of rigid body to simplify the model)
#MASS : spine link, foot
m_spine, m_contact = symbols('m_spine, m_contact')
particles = []
g_p = me.Particle('g_p', G, m_spine)
particles.append(g_p)
rc_p = me.Particle('rc_p', RC, m_contact)
particles.append(rc_p)
sg_p = me.Particle('sg_p', SG, m_spine)
particles.append(sg_p)
s1_p = me.Particle('s1_p', S1, m_spine)
particles.append(s1_p)
for part in particles:
part.potential_energy = part.mass * dot(part.point.pos_from(O), inertial_rf.z)
#### FORCES
# force = (point, vector)
# torque = (reference frame, vector)
# The location of the bobs (at the joints between the links) are created by
# specifiying the vectors between the points.
P1 = O.locatenew('P1', -l[0] * A.y)
P2 = P1.locatenew('P2', -l[1] * B.y)
P3 = P2.locatenew('P3', -l[2] * C.y)
# The velocities of the points can be computed by taking advantage that
# pairs of points are fixed on the referene frames.
P1.v2pt_theory(O, I, A)
P2.v2pt_theory(P1, I, B)
P3.v2pt_theory(P2, I, C)
points = [P1, P2, P3]
# Now create a particle to represent each bob.
Pa1 = me.Particle('Pa1', points[0], m_bob[0])
Pa2 = me.Particle('Pa2', points[1], m_bob[1])
Pa3 = me.Particle('Pa3', points[2], m_bob[2])
particles = [Pa1, Pa2, Pa3]
# The mass centers of each link need to be specified and, assuming a
# constant density cylinder, it is equidistance from each joint.
P_link1 = O.locatenew('P_link1', -l[0] / 2 * A.y)
P_link2 = P1.locatenew('P_link2', -l[1] / 2 * B.y)
P_link3 = P2.locatenew('P_link3', -l[2] / 2 * C.y)
# The linear velocities can be specified the same way as the bob points.
P_link1.v2pt_theory(O, I, A)
P_link2.v2pt_theory(P1, I, B)
P_link3.v2pt_theory(P2, I, C)
import sympy.physics.mechanics as me
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()
import sympy as _sm
import math as m
import numpy as _np
q1, q2, u1, u2 = _me.dynamicsymbols('q1 q2 u1 u2')
q1_d, q2_d, u1_d, u2_d = _me.dynamicsymbols('q1_ q2_ u1_ u2_', 1)
l, m, g = _sm.symbols('l m g', real=True)
frame_n = _me.ReferenceFrame('n')
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
def n_link_pendulum_on_cart(n,
cart_force=True,
joint_torques=False,
spring_damper=False):
"""Returns the the symbolic first order equations of motion for a 2D
n-link pendulum on a sliding cart under the influence of gravity in this
form:
M(x) x(t) = F(x, u, t)
Parameters
----------
n : integer
The number of links in the pendulum.
cart_force : boolean, default=True
If true an external specified lateral force is applied to the cart.
joint_torques : boolean, default=False
If true joint torques will be added as specified inputs at each
joint.
spring_damper : boolean, default=False
If true a linear spring and damper are added to constrain the cart
to the origin.
Returns
-------
mass_matrix : sympy.MutableMatrix, shape(2 * (n + 1), 2 * (n + 1))
The symbolic mass matrix of the system which are linear in u' and q'.
forcing_vector : sympy.MutableMatrix, shape(2 * (n + 1), 1)
The forcing vector of the system.
constants : list
A sequence of all the symbols which are constants in the equations
of motion.
coordinates : list
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the configuration of the system.
speeds : list
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the generalized speeds of the system.
specfied : list
A sequence of all the dynamic symbols, i.e. functions of time, which
describe the specified inputs to the system.
Notes
-----
The degrees of freedom of the system are n + 1, i.e. one for each
pendulum link and one for the lateral motion of the cart.
M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
The joint angles are all defined relative to the ground where the x axis
defines the ground line and the y axis points up. The joint torques are
applied between each adjacent link and the between the cart and the
lower link where a positive torque corresponds to positive angle.
"""
if n <= 0:
raise ValueError('The number of links must be a positive integer.')
q = me.dynamicsymbols('q:{}'.format(n + 1))
u = me.dynamicsymbols('u:{}'.format(n + 1))
if joint_torques is True:
T = me.dynamicsymbols('T1:{}'.format(n + 1))
m = sym.symbols('m:{}'.format(n + 1))
l = sym.symbols('l:{}'.format(n))
g, t = sym.symbols('g t')
I = me.ReferenceFrame('I')
O = me.Point('O')
O.set_vel(I, 0)
P0 = me.Point('P0')
P0.set_pos(O, q[0] * I.x)
P0.set_vel(I, u[0] * I.x)
Pa0 = me.Particle('Pa0', P0, m[0])
frames = [I]
points = [P0]
particles = [Pa0]
if spring_damper:
k, c = sym.symbols('k, c')
forces = [(P0, -m[0] * g * I.y - k * q[0] * I.x - c * u[0] * I.x)]
else:
forces = [(P0, -m[0] * g * I.y)]
kindiffs = [q[0].diff(t) - u[0]]
if cart_force is True or joint_torques is True:
specified = []
else:
specified = None
for i in range(n):
Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
Bi.set_ang_vel(I, u[i + 1] * I.z)
frames.append(Bi)
Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
Pi.v2pt_theory(points[-1], I, Bi)
points.append(Pi)
Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
particles.append(Pai)
forces.append((Pi, -m[i + 1] * g * I.y))
if joint_torques is True:
specified.append(T[i])
if i == 0:
forces.append((I, -T[i] * I.z))
if i == n - 1:
forces.append((Bi, T[i] * I.z))
else:
forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))
kindiffs.append(q[i + 1].diff(t) - u[i + 1])
if cart_force is True:
F = me.dynamicsymbols('F')
forces.append((P0, F * I.x))
specified.append(F)
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
kane.kanes_equations(particles, forces)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
coordinates = [x for x in kane._q]
speeds = [x for x in kane._u]
if spring_damper:
constants = [k, c, g, m[0]]
else:
constants = [g, m[0]]
for i in range(n):
constants += [l[i], m[i + 1]]
return (mass_matrix, forcing_vector, constants, coordinates, speeds,
specified)
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
q1, q2 = me.dynamicsymbols('q1 q2')
q1d, q2d = me.dynamicsymbols('q1 q2', 1)
q1d2, q2d2 = me.dynamicsymbols('q1 q2', 2)
l, m, g = sm.symbols('l m g', real=True)
frame_n = me.ReferenceFrame('n')
point_pn = me.Point('pn')
point_pn.set_vel(frame_n, 0)
theta1 = sm.atan(q2 / q1)
frame_a = me.ReferenceFrame('a')
frame_a.orient(frame_n, 'Axis', [theta1, frame_n.z])
particle_p = me.Particle('p', me.Point('p_pt'), sm.Symbol('m'))
particle_p.point.set_pos(point_pn, q1 * frame_n.x + q2 * frame_n.y)
particle_p.mass = m
particle_p.point.set_vel(frame_n,
(point_pn.pos_from(particle_p.point)).dt(frame_n))
f_v = me.dot((particle_p.point.vel(frame_n)).express(frame_a), frame_a.x)
dependent = sm.Matrix([[0]])
dependent[0] = f_v
velocity_constraints = [i for i in dependent]
u_q1d = me.dynamicsymbols('u_q1d')
u_q2d = me.dynamicsymbols('u_q2d')
kd_eqs = [q1d - u_q1d, q2d - u_q2d]
forceList = [(particle_p.point, particle_p.mass * (g * frame_n.x))]
kane = me.KanesMethod(frame_n,
q_ind=[q1, q2],
u_ind=[u_q2d],
import sympy.physics.mechanics as me
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(
def integrate_pendulum(n,
times,
initial_positions=135,
initial_velocities=0,
lengths=None,
masses=1):
"""Integrate a multi-pendulum with `n` sections"""
# -------------------------------------------------
# Step 1: construct the pendulum model
# Generalized coordinates and velocities
# (in this case, angular positions & velocities of each mass)
q = mechanics.dynamicsymbols('q:{0}'.format(n))
u = mechanics.dynamicsymbols('u:{0}'.format(n))
# mass and length
m = symbols('m:{0}'.format(n))
l = symbols('l:{0}'.format(n))
# gravity and time symbols
g, t = symbols('g,t')
# --------------------------------------------------
# Step 2: build the model using Kane's Method
# Create pivot point reference frame
A = mechanics.ReferenceFrame('A')
P = mechanics.Point('P')
P.set_vel(A, 0)
# lists to hold particles, forces, and kinetic ODEs
# for each pendulum in the chain
particles = []
forces = []
kinetic_odes = []
for i in range(n):
# Create a reference frame following the i^th mass
Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z])
Ai.set_ang_vel(A, u[i] * A.z)
# Create a point in this reference frame
Pi = P.locatenew('P' + str(i), l[i] * Ai.x)
Pi.v2pt_theory(P, A, Ai)
# Create a new particle of mass m[i] at this point
Pai = mechanics.Particle('Pa' + str(i), Pi, m[i])
particles.append(Pai)
# Set forces & compute kinematic ODE
forces.append((Pi, m[i] * g * A.x))
kinetic_odes.append(q[i].diff(t) - u[i])
P = Pi
# Generate equations of motion
KM = mechanics.KanesMethod(A, q_ind=q, u_ind=u, kd_eqs=kinetic_odes)
fr, fr_star = KM.kanes_equations(forces, particles)
# -----------------------------------------------------
# Step 3: numerically evaluate equations and integrate
# initial positions and velocities – assumed to be given in degrees
y0 = np.deg2rad(
np.concatenate([
np.broadcast_to(initial_positions, n),
np.broadcast_to(initial_velocities, n)
]))
# lengths and masses
if lengths is None:
lengths = np.ones(n) / n
lengths = np.broadcast_to(lengths, n)
masses = np.broadcast_to(masses, n)
# Fixed parameters: gravitational constant, lengths, and masses
parameters = [g] + list(l) + list(m)
parameter_vals = [9.81] + list(lengths) + list(masses)
# define symbols for unknown parameters
unknowns = [Dummy() for i in q + u]
unknown_dict = dict(zip(q + u, unknowns))
kds = KM.kindiffdict()
# substitute unknown symbols for qdot terms
mm_sym = KM.mass_matrix_full.subs(kds).subs(unknown_dict)
fo_sym = KM.forcing_full.subs(kds).subs(unknown_dict)
# create functions for numerical calculation
mm_func = lambdify(unknowns + parameters, mm_sym)
fo_func = lambdify(unknowns + parameters, fo_sym)
# function which computes the derivatives of parameters
def gradient(y, t, args):
vals = np.concatenate((y, args))
sol = np.linalg.solve(mm_func(*vals), fo_func(*vals))
return np.array(sol).T[0]
# ODE integration
return odeint(gradient, y0, times, args=(parameter_vals, ))
frame_a = _me.ReferenceFrame('a')
frame_b = _me.ReferenceFrame('b')
frame_n = _me.ReferenceFrame('n')
x1, x2, x3 = _me.dynamicsymbols('x1 x2 x3')
l = _sm.symbols('l', real=True)
v1 = x1*frame_a.x+x2*frame_a.y+x3*frame_a.z
v2 = x1*frame_b.x+x2*frame_b.y+x3*frame_b.z
v3 = x1*frame_n.x+x2*frame_n.y+x3*frame_n.z
v = v1+v2+v3
point_c = _me.Point('c')
point_d = _me.Point('d')
point_po1 = _me.Point('po1')
point_po2 = _me.Point('po2')
point_po3 = _me.Point('po3')
particle_l = _me.Particle('l', _me.Point('l_pt'), _sm.Symbol('m'))
particle_p1 = _me.Particle('p1', _me.Point('p1_pt'), _sm.Symbol('m'))
particle_p2 = _me.Particle('p2', _me.Point('p2_pt'), _sm.Symbol('m'))
particle_p3 = _me.Particle('p3', _me.Point('p3_pt'), _sm.Symbol('m'))
body_s_cm = _me.Point('s_cm')
body_s_cm.set_vel(frame_n, 0)
body_s_f = _me.ReferenceFrame('s_f')
body_s = _me.RigidBody('s', body_s_cm, body_s_f, _sm.symbols('m'), (_me.outer(body_s_f.x,body_s_f.x),body_s_cm))
body_r1_cm = _me.Point('r1_cm')
body_r1_cm.set_vel(frame_n, 0)
body_r1_f = _me.ReferenceFrame('r1_f')
body_r1 = _me.RigidBody('r1', body_r1_cm, body_r1_f, _sm.symbols('m'), (_me.outer(body_r1_f.x,body_r1_f.x),body_r1_cm))
body_r2_cm = _me.Point('r2_cm')
body_r2_cm.set_vel(frame_n, 0)
body_r2_f = _me.ReferenceFrame('r2_f')
body_r2 = _me.RigidBody('r2', body_r2_cm, body_r2_f, _sm.symbols('m'), (_me.outer(body_r2_f.x,body_r2_f.x),body_r2_cm))
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)
]).reshape(m2.shape[0], m2.shape[1])
frame_n = me.ReferenceFrame('n')
frame_a = me.ReferenceFrame('a')
frame_a.orient(frame_n, 'Axis', [x, frame_n.x])
frame_a.orient(frame_n, 'Axis', [sm.pi / 2, frame_n.x])
c1, c2, c3 = sm.symbols('c1 c2 c3', real=True)
v = c1 * frame_a.x + c2 * frame_a.y + c3 * frame_a.z
point_o = me.Point('o')
point_p = me.Point('p')
point_o.set_pos(point_p, c1 * frame_a.x)
v = (v).express(frame_n)
point_o.set_pos(point_p, (point_o.pos_from(point_p)).express(frame_n))
frame_a.set_ang_vel(frame_n, c3 * frame_a.z)
print(frame_n.ang_vel_in(frame_a))
point_p.v2pt_theory(point_o, frame_n, frame_a)
particle_p1 = me.Particle('p1', me.Point('p1_pt'), sm.Symbol('m'))
particle_p2 = me.Particle('p2', me.Point('p2_pt'), sm.Symbol('m'))
particle_p2.point.v2pt_theory(particle_p1.point, frame_n, frame_a)
point_p.a2pt_theory(particle_p1.point, frame_n, frame_a)
body_b1_cm = me.Point('b1_cm')
body_b1_cm.set_vel(frame_n, 0)
body_b1_f = me.ReferenceFrame('b1_f')
body_b1 = me.RigidBody('b1', body_b1_cm, body_b1_f, sm.symbols('m'),
(me.outer(body_b1_f.x, body_b1_f.x), body_b1_cm))
body_b2_cm = me.Point('b2_cm')
body_b2_cm.set_vel(frame_n, 0)
body_b2_f = me.ReferenceFrame('b2_f')
body_b2 = me.RigidBody('b2', body_b2_cm, body_b2_f, sm.symbols('m'),
(me.outer(body_b2_f.x, body_b2_f.x), body_b2_cm))
g = sm.symbols('g', real=True)
force_p1 = particle_p1.mass * (g * frame_n.x)
def multi_mass_spring_damper(n=1,
apply_gravity=False,
apply_external_forces=False):
"""Returns a system containing the symbolic equations of motion and
associated variables for a simple mutli-degree of freedom point mass,
spring, damper system with optional gravitational and external
specified forces. For example, a two mass system under the influence of
gravity and external forces looks like:
::
----------------
| | | | g
\ | | | V
k0 / --- c0 |
| | | x0, v0
--------- V
| m0 | -----
--------- |
| | | |
\ v | | |
k1 / f0 --- c1 |
| | | x1, v1
--------- V
| m1 | -----
---------
| f1
V
Parameters
----------
n : integer
The number of masses in the serial chain.
apply_gravity : boolean
If true, gravity will be applied to each mass.
apply_external_forces : boolean
If true, a time varying external force will be applied to each mass.
Returns
-------
kane : sympy.physics.mechanics.kane.KanesMethod
A KanesMethod object.
"""
mass = sm.symbols('m:{}'.format(n))
stiffness = sm.symbols('k:{}'.format(n))
damping = sm.symbols('c:{}'.format(n))
acceleration_due_to_gravity = sm.symbols('g')
coordinates = me.dynamicsymbols('x:{}'.format(n))
speeds = me.dynamicsymbols('v:{}'.format(n))
specifieds = me.dynamicsymbols('f:{}'.format(n))
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
points = [origin]
kinematic_equations = []
particles = []
forces = []
for i in range(n):
center = points[-1].locatenew('center{}'.format(i),
coordinates[i] * ceiling.x)
center.set_vel(ceiling,
points[-1].vel(ceiling) + speeds[i] * ceiling.x)
points.append(center)
block = me.Particle('block{}'.format(i), center, mass[i])
kinematic_equations.append(speeds[i] - coordinates[i].diff())
total_force = (-stiffness[i] * coordinates[i] - damping[i] * speeds[i])
try:
total_force += (stiffness[i + 1] * coordinates[i + 1] +
damping[i + 1] * speeds[i + 1])
except IndexError: # no force from below on last mass
pass
if apply_gravity:
total_force += mass[i] * acceleration_due_to_gravity
if apply_external_forces:
total_force += specifieds[i]
forces.append((center, total_force * ceiling.x))
particles.append(block)
kane = me.KanesMethod(ceiling,
q_ind=coordinates,
u_ind=speeds,
kd_eqs=kinematic_equations)
kane.kanes_equations(particles, forces)
return kane
def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False):
"""Returns the system containing the symbolic first order equations of
motion for a 2D n-link pendulum on a sliding cart under the influence of
gravity.
::
|
o y v
\ 0 ^ g
\ |
--\-|----
| \| |
F-> | o --|---> x
| |
---------
o o
Parameters
----------
n : integer
The number of links in the pendulum.
cart_force : boolean, default=True
If true an external specified lateral force is applied to the cart.
joint_torques : boolean, default=False
If true joint torques will be added as specified inputs at each
joint.
Returns
-------
kane : sympy.physics.mechanics.kane.KanesMethod
A KanesMethod object.
Notes
-----
The degrees of freedom of the system are n + 1, i.e. one for each
pendulum link and one for the lateral motion of the cart.
M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
The joint angles are all defined relative to the ground where the x axis
defines the ground line and the y axis points up. The joint torques are
applied between each adjacent link and the between the cart and the
lower link where a positive torque corresponds to positive angle.
"""
if n <= 0:
raise ValueError('The number of links must be a positive integer.')
q = me.dynamicsymbols('q:{}'.format(n + 1))
u = me.dynamicsymbols('u:{}'.format(n + 1))
if joint_torques is True:
T = me.dynamicsymbols('T1:{}'.format(n + 1))
m = sm.symbols('m:{}'.format(n + 1))
l = sm.symbols('l:{}'.format(n))
g, t = sm.symbols('g t')
I = me.ReferenceFrame('I')
O = me.Point('O')
O.set_vel(I, 0)
P0 = me.Point('P0')
P0.set_pos(O, q[0] * I.x)
P0.set_vel(I, u[0] * I.x)
Pa0 = me.Particle('Pa0', P0, m[0])
frames = [I]
points = [P0]
particles = [Pa0]
forces = [(P0, -m[0] * g * I.y)]
kindiffs = [q[0].diff(t) - u[0]]
if cart_force is True or joint_torques is True:
specified = []
else:
specified = None
for i in range(n):
Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
Bi.set_ang_vel(I, u[i + 1] * I.z)
frames.append(Bi)
Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
Pi.v2pt_theory(points[-1], I, Bi)
points.append(Pi)
Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
particles.append(Pai)
forces.append((Pi, -m[i + 1] * g * I.y))
if joint_torques is True:
specified.append(T[i])
if i == 0:
forces.append((I, -T[i] * I.z))
if i == n - 1:
forces.append((Bi, T[i] * I.z))
else:
forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))
kindiffs.append(q[i + 1].diff(t) - u[i + 1])
if cart_force is True:
F = me.dynamicsymbols('F')
forces.append((P0, F * I.x))
specified.append(F)
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
kane.kanes_equations(particles, forces)
return kane
import sympy.physics.mechanics as _me
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()
import sympy.physics.mechanics as _me
import sympy as _sm
import math as m
import numpy as _np
frame_a = _me.ReferenceFrame('a')
c1, c2, c3 = _sm.symbols('c1 c2 c3', real=True)
a = _me.inertia(frame_a, 1, 1, 1)
particle_p1 = _me.Particle('p1', _me.Point('p1_pt'), _sm.Symbol('m'))
particle_p2 = _me.Particle('p2', _me.Point('p2_pt'), _sm.Symbol('m'))
body_r_cm = _me.Point('r_cm')
body_r_f = _me.ReferenceFrame('r_f')
body_r = _me.RigidBody('r', body_r_cm, body_r_f, _sm.symbols('m'), (_me.outer(body_r_f.x,body_r_f.x),body_r_cm))
frame_a.orient(body_r_f, 'DCM', _sm.Matrix([1,1,1,1,1,0,0,0,1]).reshape(3, 3))
point_o = _me.Point('o')
m1 = _sm.symbols('m1')
particle_p1.mass = m1
m2 = _sm.symbols('m2')
particle_p2.mass = m2
mr = _sm.symbols('mr')
body_r.mass = mr
i1 = _sm.symbols('i1')
i2 = _sm.symbols('i2')
i3 = _sm.symbols('i3')
body_r.inertia = (_me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm)
point_o.set_pos(particle_p1.point, c1*frame_a.x)
point_o.set_pos(particle_p2.point, c2*frame_a.y)
point_o.set_pos(body_r_cm, c3*frame_a.z)
a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a)
a = _me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a)
a = body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
frame_n = me.ReferenceFrame("n")
frame_a = me.ReferenceFrame("a")
a = 0
d = me.inertia(frame_a, 1, 1, 1)
point_po1 = me.Point("po1")
point_po2 = me.Point("po2")
particle_p1 = me.Particle("p1", me.Point("p1_pt"), sm.Symbol("m"))
particle_p2 = me.Particle("p2", me.Point("p2_pt"), sm.Symbol("m"))
c1, c2, c3 = me.dynamicsymbols("c1 c2 c3")
c1d, c2d, c3d = me.dynamicsymbols("c1 c2 c3", 1)
body_r_cm = me.Point("r_cm")
body_r_cm.set_vel(frame_n, 0)
body_r_f = me.ReferenceFrame("r_f")
body_r = me.RigidBody(
"r",
body_r_cm,
body_r_f,
sm.symbols("m"),
(me.outer(body_r_f.x, body_r_f.x), body_r_cm),
)
point_po2.set_pos(particle_p1.point, c1 * frame_a.x)
v = 2 * point_po2.pos_from(particle_p1.point) + c2 * frame_a.y
frame_a.set_ang_vel(frame_n, c3 * frame_a.z)
v = 2 * frame_a.ang_vel_in(frame_n) + c2 * frame_a.y
body_r_f.set_ang_vel(frame_n, c3 * frame_a.z)
v = 2 * body_r_f.ang_vel_in(frame_n) + c2 * frame_a.y
def __init__(self, n, lengths=None, masses=1):
#-------------------------------------------------
# Step 1: construct the pendulum model
# Generalized coordinates and velocities
# (in this case, angular positions & velocities of each mass)
q = mechanics.dynamicsymbols('q:{0}'.format(n))
u = mechanics.dynamicsymbols('u:{0}'.format(n))
f = mechanics.dynamicsymbols('f:{0}'.format(n))
# mass and length
m = symbols('m:{0}'.format(n))
l = symbols('l:{0}'.format(n))
# gravity and time symbols
g, t = symbols('g,t')
#--------------------------------------------------
# Step 2: build the model using Kane's Method
# Create pivot point reference frame
A = mechanics.ReferenceFrame('A')
P = mechanics.Point('P')
P.set_vel(A, 0)
# lists to hold particles, forces, and kinetic ODEs
# for each pendulum in the chain
particles = []
forces = []
kinetic_odes = []
for i in range(n):
# Create a reference frame following the i^th mass
Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z])
Ai.set_ang_vel(A, u[i] * A.z)
# Create a point in this reference frame
Pi = P.locatenew('P' + str(i), l[i] * Ai.x)
Pi.v2pt_theory(P, A, Ai)
# Create a new particle of mass m[i] at this point
Pai = mechanics.Particle('Pa' + str(i), Pi, m[i])
particles.append(Pai)
# Set forces & compute kinematic ODE
forces.append((Pi, m[i] * g * A.x))
# Add external torque:
forces.append((Ai, f[i] * A.z))
kinetic_odes.append(q[i].diff(t) - u[i])
P = Pi
# Generate equations of motion
KM = mechanics.KanesMethod(A, q_ind=q, u_ind=u, kd_eqs=kinetic_odes)
fr, fr_star = KM.kanes_equations(particles, forces)
#-----------------------------------------------------
# Step 3: numerically evaluate equations
# lengths and masses
if lengths is None:
lengths = np.ones(n) / n
lengths = np.broadcast_to(lengths, n)
masses = np.broadcast_to(masses, n)
# Fixed parameters: gravitational constant, lengths, and masses
parameters = [g] + list(l) + list(m)
parameter_vals = [9.81] + list(lengths) + list(masses)
# define symbols for unknown parameters
unknowns = [Dummy() for i in q + u + f]
unknown_dict = dict(zip(q + u + f, unknowns))
kds = KM.kindiffdict()
# substitute unknown symbols for qdot terms
mm_sym = KM.mass_matrix_full.subs(kds).subs(unknown_dict)
fo_sym = KM.forcing_full.subs(kds).subs(unknown_dict)
# create functions for numerical calculation
self.mm_func = lambdify(unknowns + parameters, mm_sym)
self.fo_func = lambdify(unknowns + parameters, fo_sym)
self.args = parameter_vals
A, B, _ = KM.linearize(A_and_B=True)
parameter_dict = dict(zip(parameters, parameter_vals))
self.A = A.subs(parameter_dict)
self.B = B.subs(parameter_dict)
self.state = q + u
def setup(self):
# System state variables
q = me.dynamicsymbols('q')
qd = me.dynamicsymbols('q', 1)
# Other system variables
m, k, b = symbols('m k b')
# Set up the reference frame
N = me.ReferenceFrame('N')
# Set up the points and particles
P = me.Point('P')
P.set_vel(N, qd * N.x)
Pa = me.Particle('Pa', P, m)
# Define the potential energy and create the Lagrangian
Pa.potential_energy = k * q**2 / 2.0
L = me.Lagrangian(N, Pa)
# Create the list of forces acting on the system
fl = [(P, -b * qd * N.x)]
# Create an instance of Lagranges Method
self.l = me.LagrangesMethod(L, [q], forcelist=fl, frame=N)