def _create_reference_frames(self):
# Set Reference Frames
self.frames = OrderedDict()
self.frames['inertial'] = me.ReferenceFrame('I')
self.frames['left_thigh'] = me.ReferenceFrame('LT')
self.frames['left_shank'] = me.ReferenceFrame('LS')
self.frames['right_thigh'] = me.ReferenceFrame('RT')
self.frames['right_shank'] = me.ReferenceFrame('RS')
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_specifying_coordinate_issue_339():
"""This test ensures that you can use derivatives as specified values."""
# beta will be a specified angle
beta = me.dynamicsymbols('beta')
q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4')
u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4')
N = me.ReferenceFrame('N')
A = N.orientnew('A', 'Axis', (q1, N.x))
B = A.orientnew('B', 'Axis', (beta, A.y))
No = me.Point('No')
Ao = No.locatenew('Ao', q2 * N.x + q3 * N.y + q4 * N.z)
Bo = Ao.locatenew('Bo', 10 * A.x + 10 * A.y + 10 * A.z)
A.set_ang_vel(N, u1 * N.x)
B.ang_vel_in(N) # compute it automatically
No.set_vel(N, 0)
Ao.set_vel(N, u2 * N.x + u3 * N.y + u4 * N.z)
Bo.v2pt_theory(Ao, N, B)
body_A = me.RigidBody('A', Ao, A, 1.0, (me.inertia(A, 1, 2, 3), Ao))
body_B = me.RigidBody('B', Bo, B, 1.0, (me.inertia(A, 3, 2, 1), Bo))
bodies = [body_A, body_B]
# TODO : This should be able to be simple an empty iterable.
loads = [(No, 0 * N.x)]
kdes = [u1 - q1.diff(), u2 - q2.diff(), u3 - q3.diff(), u4 - q4.diff()]
kane = me.KanesMethod(N,
q_ind=[q1, q2, q3, q4],
u_ind=[u1, u2, u3, u4],
kd_eqs=kdes)
if sympy_newer_than('1.0'):
fr, frstar = kane.kanes_equations(bodies, loads)
else:
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.specifieds = {
(beta, beta.diff(), beta.diff().diff()):
lambda x, t: np.array([1.0, 1.0, 1.0])
}
sys.times = np.linspace(0, 10, 20)
sys.integrate()
def test_issue_14041():
import sympy.physics.mechanics as me
A_frame = me.ReferenceFrame('A')
thetad, phid = me.dynamicsymbols('theta, phi', 1)
L = symbols('L')
assert vlatex(L*(phid + thetad)**2*A_frame.x) == \
r"L \left(\dot{\phi} + \dot{\theta}\right)^{2}\mathbf{\hat{a}_x}"
assert vlatex((phid + thetad)**2*A_frame.x) == \
r"\left(\dot{\phi} + \dot{\theta}\right)^{2}\mathbf{\hat{a}_x}"
assert vlatex((phid*thetad)**a*A_frame.x) == \
r"\left(\dot{\phi} \dot{\theta}\right)^{a}\mathbf{\hat{a}_x}"
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 test_get_angle():
bus = mech.ReferenceFrame("bus")
one = [
bus.orientnew("one", "Axis", [-30 / 180 * sp.pi, bus.z]),
-30 / 180 * sp.pi
]
three = [
bus.orientnew("three", "Axis", [-90 / 180 * sp.pi, bus.z]),
-90 / 180 * sp.pi
]
five = [
bus.orientnew("five", "Axis", [-120 / 180 * sp.pi, bus.z]),
-120 / 180 * sp.pi
]
six_cw = [
bus.orientnew("six_cw", "Axis", [-180 / 180 * sp.pi, bus.z]),
180 / 180 * sp.pi
]
six_ccw = [
bus.orientnew("six_ccw", "Axis", [180 / 180 * sp.pi, bus.z]),
180 / 180 * sp.pi
]
seven = [
bus.orientnew("seven", "Axis", [150 / 180 * sp.pi, bus.z]),
150 / 180 * sp.pi
]
nine = [
bus.orientnew("nine", "Axis", [90 / 180 * sp.pi, bus.z]),
90 / 180 * sp.pi
]
eleven = [
bus.orientnew("eleven", "Axis", [30 / 180 * sp.pi, bus.z]),
30 / 180 * sp.pi
]
tests = [one, three, six_cw, six_ccw, nine, eleven]
for i in tests:
angle = get_angle(i[0], bus)
if threshold(angle, i[1]):
pass
else:
print("Angle test did not pass: \n")
print(i[0].name)
print("Got: " + str(angle) + ", expected: " + str(i[1]))
exit()
print("All angle tests for the reference system passed.")
def biped_model_left():
# 定义状态变量
alpha, beta, gamma = me.dynamicsymbols('alp beta gama') # 与地面接触点状态
dalpha, dbeta, dgamma = me.dynamicsymbols('alp beta gama', 1)
lj1, lj2, lj3 = me.dynamicsymbols('lj1 lj2 lj3') # 左腿状态变量
dlj1, dlj2, dlj3 = me.dynamicsymbols('lj1 lj2 lj3', 1)
rj1, rj2, rj3 = me.dynamicsymbols('rj1, rj2 rj3') # 右腿状态变量
drj1, drj2, drj3 = me.dynamicsymbols('rj1, rj2 rj3', 1)
# 定义常量
# 定义坐标系
ground = me.ReferenceFrame('ground')
axis_1 = ground.orientnew('Axis1', 'Body', [alpha, beta, gamma], '123')
axis_2 = axis_1.orientnew('Axis2', 'Axis', [lj2, axis_1.y])
# 定义点
ori = me.Point('ori')
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
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()
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
axes.get_xlim()[1], 5),
np.linspace(axes.get_ylim()[0],
axes.get_ylim()[1], 5))
u = y / y * float(sp.sin(angle_to_sun).evalf())
v = x / x * float(sp.cos(angle_to_sun).evalf())
#fig, axes = plt.subplots()
axes.quiver(x, y, u, v)
axes.set_title("Deflection diagram")
axes.set_xlabel("X axis (m)")
axes.set_ylabel("Y axis (m)")
#diagram.savefig("deflection_diagram.fig")
diagram.savefig("deflection_diagram.png")
plt.show()
if __name__ == "__main__":
#test_get_angle()
global convergence_mean
convergence_mean = 300
global sun
sun = mech.ReferenceFrame("sun")
global angle_to_sun
angle_to_sun = 0 #30/180 *sp.pi
n = 1
panel_ors = {}
panel_ors, bus = main()
draw_scene(panel_ors, bus)
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)
eqn[eqn.shape[0] - 1] = 2 * a * x - 3 * b * y
print(sm.solve(eqn, x, y))
rhs_y = sm.solve(eqn, x, y)[y]
e = (x + y)**2 + 2 * x**2
e.collect(x)
a, b, c = sm.symbols('a b c', real=True)
m = sm.Matrix([a, b, c, 0]).reshape(2, 2)
m2 = sm.Matrix([i.subs({
a: 1,
b: 2,
c: 3
}) for i in m]).reshape((m).shape[0], (m).shape[1])
eigvalue = sm.Matrix([i.evalf() for i in (m2).eigenvals().keys()])
eigvec = sm.Matrix([i[2][0].evalf() for i in (m2).eigenvects()
]).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'))
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
angle and roll angle.
"""
import numpy as np
from scipy.optimize import fsolve
import matplotlib.pyplot as plt
import sympy as sym
import sympy.physics.mechanics as me
# theta is the bicycle roll angle and beta is potentiometer angle
theta, alpha, beta = sym.symbols('theta alpha beta')
rr, h, l, rt = sym.symbols('rr h l rt')
# newtonian reference frame
N = me.ReferenceFrame('N', indices=('1', '2', '3'))
# rear wheel rolls with respect to the newtonian frame
R = N.orientnew('R', 'Axis', (theta, N['1']), indices=('1', '2', '3'))
# the yolk pitches with respect to the wheel
Y = R.orientnew('Y', 'Axis', (alpha, R['2']), indices=('1', '2', '3'))
# the trailer rolls with respect to the yolk (beta is the potentiometer angle)
T = Y.orientnew('T', 'Axis', (beta, Y['1']), indices=('1', '2', '3'))
# rear wheel contact to the center of the trailer axle
rAcNo = -rr * R['3'] + h * Y['3'] - l * Y['1'] - (h + rt - rr) * T['3']
holo = []
# the axle must be the wheel radius above the ground
holo.append(rAcNo.dot(N['3']) + rt)
# the trailer must be horizontal with respect to the ground
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)
# Move the workspace down 0.5 meters
z_begin = 0.5
z_end = 3
# In[3]:
# Create the symbols
x_dot, z_dot, e_dot = sympy.symbols('x_dot z_dot e_dot')
theta_dot, phi_dot = sympy.symbols('theta_dot phi_dot')
H, a, b, M, m, g, k, t = sympy.symbols('H a b M m g k_{cable} t')
Ip, Ir, c, r, p, kr, cr, D = sympy.symbols(
'I_{plate} I_{rod} c r p k_{rod} c_{rod} D')
L1, L2, e_offset, k_C = sympy.symbols('L1 L2 e_{offset} k_{CFrame}')
# Create the frames Z+ points down, and Y+ is out of the screen
A = me.ReferenceFrame('A')
for j in range(2):
if j == 0:
x, z, e, theta, phi = sympy.symbols('x z e theta phi')
# Orient the Beta frame
B = A.orientnew('B', 'Axis', [theta, A.y])
C = A.orientnew('C', 'Axis', [phi, A.y])
B.set_ang_vel(A, theta_dot * A.y)
C.set_ang_vel(A, phi_dot * A.y)
else:
x, z, e, theta, phi = me.dynamicsymbols('x z e theta phi')
# Orient the Beta frame
B = A.orientnew('B', 'Axis', [theta, A.y])
C = A.orientnew('C', 'Axis', [phi, A.y])
B.set_ang_vel(A, theta_dot * A.y)
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)
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
left_cable_k = 100
right_cable_k = 100
rod_k = 250.0
cable_c = 10.0
rod_c = 10.0
plate_width = 4.0
plate_height = 2.0
mass_of_plate = 10.0
rod_length = 3.0
mass_of_rod = 2.0
rod_init = (9.81 * mass_of_rod) / rod_k
inertia_of_plate = (plate_width**2 + plate_height**2) * (mass_of_plate / 12.0)
inertia_of_rod = (mass_of_rod * rod_length**2) / 12.0
# Create the frames
N = me.ReferenceFrame('N')
B = me.ReferenceFrame('B')
# Create the symbols
x, y, beta, e, F, L1, L2 = me.dynamicsymbols('x y beta e F L1 L2')
x_dot, y_dot, beta_dot, e_dot = me.dynamicsymbols('x_dot y_dot beta_dot e_dot')
H, a, b, m, g, k, t = sympy.symbols('H a b m g k t')
c, c_rod, D, M, k_rod, mom = sympy.symbols('c c_rod D M k_rod mom')
Izz, Izz_rod = sympy.symbols('Izz Izz_rod')
# Orient the Beta frame
B.orient(N, 'Axis', (beta, N.z))
B.set_ang_vel(N, beta_dot * N.z)
# Create the first point
O1 = me.Point('O1')
def _create_reference_frames(self):
self.frames = OrderedDict()
self.frames['inertial'] = me.ReferenceFrame('I')
self.frames['leg'] = me.ReferenceFrame('L')
self.frames['torso'] = me.ReferenceFrame('T')
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
from pydy.system import System
import pydy
q = me.dynamicsymbols('q:12')
u = me.dynamicsymbols('u0:12')
qd = me.dynamicsymbols('q:12', 1)
ud = me.dynamicsymbols('u0:12', 1)
L = sm.symbols('LA:D0:4x:z')
I = sm.symbols('Ia:bx:z')
g = sm.symbols('g')
ma, mb = sm.symbols('ma:b')
k = sm.symbols('k0:2x:z')
Lgr, Lsp = sm.symbols('Lgr Lsp')
N = me.ReferenceFrame('N')
Orig = me.Point('O')
Orig.set_vel(N, 0 * N.x + 0 * N.y + 0 * N.z)
OA = [
Orig.locatenew('OA_0', L[0] * N.x + L[1] * N.y + L[2] * N.z),
Orig.locatenew('OA_1', -L[3] * N.x + L[4] * N.y + L[5] * N.z),
Orig.locatenew('OA_2', -L[6] * N.x + L[7] * N.y - L[8] * N.z),
Orig.locatenew('OA_3', L[9] * N.x + L[10] * N.y - L[11] * N.z)
]
A = N.orientnew('A', 'Body', (q[0], q[1], q[2]), '123')
A.set_ang_vel(N, u[0] * N.x + u[1] * N.y + u[2] * N.z)
CMa = Orig.locatenew('CM_s', q[3] * N.x + q[4] * N.y + q[5] * N.z)
CMa.set_vel(N, u[3] * N.x + u[4] * N.y + u[5] * N.z)
AO = [
CMa.locatenew('AO_0', L[12] * A.x - L[13] * A.y + L[14] * A.z),
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
g, lb, w, h = sm.symbols('g lb w h', real=True)
theta, phi, omega, alpha = me.dynamicsymbols('theta phi omega alpha')
thetad, phid, omegad, alphad = me.dynamicsymbols('theta phi omega alpha', 1)
thetad2, phid2 = me.dynamicsymbols('theta phi', 2)
frame_n = me.ReferenceFrame('n')
body_a_cm = me.Point('a_cm')
body_a_cm.set_vel(frame_n, 0)
body_a_f = me.ReferenceFrame('a_f')
body_a = me.RigidBody('a', body_a_cm, body_a_f, sm.symbols('m'), (me.outer(body_a_f.x,body_a_f.x),body_a_cm))
body_b_cm = me.Point('b_cm')
body_b_cm.set_vel(frame_n, 0)
body_b_f = me.ReferenceFrame('b_f')
body_b = me.RigidBody('b', body_b_cm, body_b_f, sm.symbols('m'), (me.outer(body_b_f.x,body_b_f.x),body_b_cm))
body_a_f.orient(frame_n, 'Axis', [theta, frame_n.y])
body_b_f.orient(body_a_f, 'Axis', [phi, body_a_f.z])
point_o = me.Point('o')
la = (lb-h/2)/2
body_a_cm.set_pos(point_o, la*body_a_f.z)
body_b_cm.set_pos(point_o, lb*body_a_f.z)
body_a_f.set_ang_vel(frame_n, omega*frame_n.y)
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
"""
import sympy
import sympy.physics.mechanics as mech
# Set dynamics variables
phi = mech.dynamicsymbols('theta')
# Blade angle
phiDot = mech.dynamicsymbols('thetaDot')
# Blade angluar velocity
# Create symboloic variables we'll need
m, L, kT, Omega, epsilon, Izz = sympy.symbols('m L kT Omega epsilon Izz')
# Create inertial frame
inertialFrame = mech.ReferenceFrame('inertialFrame')
# Create reference frame at A that rotates with Omega
N = mech.ReferenceFrame('N')
N.set_ang_vel(inertialFrame, -Omega * inertialFrame.z)
# Set point A
A = mech.Point('A')
A.set_vel(N, 0)
# Set point B
B = mech.Point('B')
B.set_vel(N, 0)
# B has no velocity in the rotating frame N
B.set_pos(A, epsilon * N.x)
# phi measured from x-axis along AB
x, y = _me.dynamicsymbols('x y')
x_d, y_d = _me.dynamicsymbols('x_ y_', 1)
e = _sm.cos(x)+_sm.sin(x)+_sm.tan(x)+_sm.cosh(x)+_sm.sinh(x)+_sm.tanh(x)+_sm.acos(x)+_sm.asin(x)+_sm.atan(x)+_sm.log(x)+_sm.exp(x)+_sm.sqrt(x)+_sm.factorial(x)+_sm.ceiling(x)+_sm.floor(x)+_sm.sign(x)
e = (x)**2+_sm.log(x, 10)
a = _sm.Abs(-1*1)+int(1.5)+round(1.9)
e1 = 2*x+3*y
e2 = x+y
am = _sm.Matrix([e1.expand().coeff(x), e1.expand().coeff(y), e2.expand().coeff(x), e2.expand().coeff(y)]).reshape(2, 2)
b = (e1).expand().coeff(x)
c = (e2).expand().coeff(y)
d1 = (e1).collect(x).coeff(x,0)
d2 = (e1).collect(x).coeff(x,1)
fm = _sm.Matrix([i.collect(x)for i in _sm.Matrix([e1,e2]).reshape(1, 2)]).reshape((_sm.Matrix([e1,e2]).reshape(1, 2)).shape[0], (_sm.Matrix([e1,e2]).reshape(1, 2)).shape[1])
f = (e1).collect(y)
g = (e1).subs({x:2*x})
gm = _sm.Matrix([i.subs({x:3}) for i in _sm.Matrix([e1,e2]).reshape(2, 1)]).reshape((_sm.Matrix([e1,e2]).reshape(2, 1)).shape[0], (_sm.Matrix([e1,e2]).reshape(2, 1)).shape[1])
frame_a = _me.ReferenceFrame('a')
frame_b = _me.ReferenceFrame('b')
theta = _me.dynamicsymbols('theta')
frame_b.orient(frame_a, 'Axis', [theta, frame_a.z])
v1 = 2*frame_a.x-3*frame_a.y+frame_a.z
v2 = frame_b.x+frame_b.y+frame_b.z
a = _me.dot(v1, v2)
bm = _sm.Matrix([_me.dot(v1, v2),_me.dot(v1, 2*v2)]).reshape(2, 1)
c = _me.cross(v1, v2)
d = 2*v1.magnitude()+3*v1.magnitude()
dyadic = _me.outer(3*frame_a.x, frame_a.x)+_me.outer(frame_a.y, frame_a.y)+_me.outer(2*frame_a.z, frame_a.z)
am = (dyadic).to_matrix(frame_b)
m = _sm.Matrix([1,2,3]).reshape(3, 1)
v = m[0]*frame_a.x +m[1]*frame_a.y +m[2]*frame_a.z
#-----------------------------------------------------------------------#
# time varying signals that can be calculated from the measured signals #
#-----------------------------------------------------------------------#
# steer
deltad, wg3d = me.dynamicsymbols('delta wg3', 1)
# body fixed angular acceleration
wb1d, wb2d, wb3d = me.dynamicsymbols('wb1 wb2 wb3', 1)
#-----------------------------#
# define the reference frames #
#-----------------------------#
# newtonian frame
N = me.ReferenceFrame('N', indices=('1', '2', '3'))
# bicycle frame
B = me.ReferenceFrame('B', indices=('1', '2', '3'))
# handlebar frame
G = me.ReferenceFrame('G', indices=('1', '2', '3'))
G.orient(B, 'Axis', (delta, B['3']))
#------------------------------------#
# define the locations of the points #
#------------------------------------#
# vectornav center
v = me.Point('v')
# point on the steer axis
s = me.Point('s')
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, ))
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)
