def test_solve_accelerations(self):
# solve_accelerations() should find:
# (a) response of system to forces (here, gravity)
# (b) include any prescribed accelerations in qdd vector
g = 9.81
s = System(gravity=g)
j = FreeJoint('joint')
b = RigidBody('body', mass=23.54, inertia=52.1 * np.eye(3))
s.add_leaf(j)
j.add_leaf(b)
s.setup()
# Prescribe horizontal acceleration. Vertical acceleration
# should result from gravity.
s.prescribe(j, 2.3, part=[0]) # x acceleration
# Initially accelerations are zero
assert_aae(j.ap, 0)
assert_aae(j.ad, 0)
assert_aae(j.astrain, 0)
# Solve accelerations & check
s.solve_accelerations()
s.update_kinematics()
assert_aae(j.ap, 0) # ground
assert_aae(j.ad, [2.3, 0, -g, 0, 0, 0])
assert_aae(j.astrain, j.ad) # not always true, but works for FreeJoint
def test_single_rigid_body(self):
mass = 36.2
inertia = np.diag([75.4, 653, 234])
s = System()
j = FreeJoint('joint')
b = RigidBody('body', mass, inertia)
s.add_leaf(j)
j.add_leaf(b)
s.setup()
# Calculate reduced system to get rigid body matrices
rsys = ReducedSystem(s)
self.assertEqual(rsys.M.shape, (6, 6))
self.assertEqual(rsys.Q.shape, (6,))
assert_aae(rsys.M[:3, :3], mass * np.eye(3))
assert_aae(rsys.M[3:, 3:], inertia)
assert_aae(rsys.M[3:, :3], 0)
assert_aae(rsys.M[:3, 3:], 0)
assert_aae(rsys.Q, 0)
# Now if some freedoms are prescribed, don't appear in reduced system
s.prescribe(j, part=[1, 2, 3, 4, 5]) # only x-translation is free now
rsys = ReducedSystem(s)
self.assertEqual(rsys.M.shape, (1, 1))
self.assertEqual(rsys.Q.shape, (1,))
assert_aae(rsys.M[0, 0], mass)
assert_aae(rsys.Q, 0)
def test_solve_accelerations_coupling(self):
# Further to test above, check that coupling between prescribed
# accelerations and other dofs is correct. For example, if there
# is a rigid body vertically offset from the joint, then a
# prescribed horizontal acceleration should cause an angular
# acceleration as well as the translational acceleration.
s = System()
j = FreeJoint('joint')
c = RigidConnection('conn', [0, 0, 1.7])
b = RigidBody('body', mass=23.54, inertia=74.1 * np.eye(3))
s.add_leaf(j)
j.add_leaf(c)
c.add_leaf(b)
s.setup()
# Prescribe horizontal acceleration, solve other accelerations
s.prescribe(j, 2.3, part=[0]) # x acceleration
s.update_kinematics() # update system to show prescribed acc
s.solve_accelerations() # solve free accelerations
s.update_kinematics() # update system to show solution
# Ground shouldn't move
assert_aae(j.ap, 0)
# Need angular acceleration = (m a_x L) / I0
I0 = 74.1 + (23.54 * 1.7**2)
expected_angular_acc = -(23.54 * 2.3 * 1.7) / I0
assert_aae(j.ad, [2.3, 0, 0, 0, expected_angular_acc, 0])
assert_aae(j.astrain, j.ad) # not always true, but works for FreeJoint
def test_simple_prescribed_integration(self):
s = System()
h = Hinge('hinge', [0, 1, 0])
s.add_leaf(h)
s.setup()
s.prescribe(h)
w = h.vstrain[0] = 0.97 # rad/s
integ = Integrator(s)
t, y = integ.integrate(9.0, 0.1)
# Check time vector and shape of result
assert_array_equal(t, np.arange(0, 9.0, 0.1))
self.assertEqual(len(y), 1)
self.assertEqual(y[0].shape, (len(t), 1))
# Result should be y = wt, but wrapped to [0, 2pi)
assert_aae(y[0][:, 0], (w * t) % (2 * np.pi))
# Check asking for velocity and acceleration works
h.xstrain[0] = s.time = 0.0 # reset
integ = Integrator(s, ('pos', 'vel', 'acc'))
t, y = integ.integrate(1.0, 0.1)
assert_array_equal(t, np.arange(0, 1.0, 0.1))
self.assertEqual(len(y), 3)
for yy in y:
self.assertEqual(yy.shape, (len(t), 1))
assert_aae(y[0][:, 0], w * t)
assert_aae(y[1][:, 0], w)
assert_aae(y[2][:, 0], 0)
def test_dof_index(self):
s = System()
j1 = FreeJoint('j1')
j2 = FreeJoint('j2')
s.add_leaf(j1)
j1.add_leaf(j2)
s.setup()
# Prescribe some of the strains:
# _____j1____ _____j2____
# Strain: 0 1 2 3 4 5 0 1 2 3 4 5
# Prescr: * * *
# Dofs: 0 1 2 3 4 5 6 7 8
s.prescribe(j1, 0, part=[1, 4])
s.prescribe(j2, 0, part=[3])
self.assertEqual(s.dof_index('j1', 0), 0)
self.assertEqual(s.dof_index('j1', 2), 1)
self.assertEqual(s.dof_index('j1', 5), 3)
self.assertEqual(s.dof_index('j2', 5), 8)
# Strain index out of range should give IndexError
with self.assertRaises(IndexError):
s.dof_index('j1', 6)
# Asking for index of prescribed strain should give ValueError
with self.assertRaises(ValueError):
s.dof_index('j1', 1)
def test_nonzero_prescribed_acceleration(self):
# Test reduction where a prescribed acceleration is non-zero:
# two sliders in series, with a mass on the end. If the second
# slider's acceleration is prescribed, the first slider's DOF
# sees an inertial force corresponding to the acceleration of
# the mass.
mass = 36.2
s = System()
s1 = PrismaticJoint('slider1', [1, 0, 0])
s2 = PrismaticJoint('slider2', [1, 0, 0])
b = RigidBody('body', mass)
s.add_leaf(s1)
s1.add_leaf(s2)
s2.add_leaf(b)
s.setup()
s.prescribe(s2, acc=0)
# With hinge angle = 0, no generalised inertial force
rsys = ReducedSystem(s)
assert_aae(rsys.M, mass)
assert_aae(rsys.Q, 0)
# With hinge angle = 90deg, do see generalised inertial force
s.prescribe(s2, acc=2.3)
rsys = ReducedSystem(s)
assert_aae(rsys.M, mass)
assert_aae(rsys.Q, -mass * 2.3)
def test_nonzero_prescribed_acceleration(self):
# Test reduction where a prescribed acceleration is non-zero:
# two sliders in series, with a mass on the end. If the second
# slider's acceleration is prescribed, the first slider's DOF
# sees an inertial force corresponding to the acceleration of
# the mass.
mass = 36.2
s = System()
s1 = PrismaticJoint('slider1', [1, 0, 0])
s2 = PrismaticJoint('slider2', [1, 0, 0])
b = RigidBody('body', mass)
s.add_leaf(s1)
s1.add_leaf(s2)
s2.add_leaf(b)
s.setup()
s.prescribe(s2, acc=0)
# With hinge angle = 0, no generalised inertial force
rsys = ReducedSystem(s)
assert_aae(rsys.M, mass)
assert_aae(rsys.Q, 0)
# With hinge angle = 90deg, do see generalised inertial force
s.prescribe(s2, acc=2.3)
rsys = ReducedSystem(s)
assert_aae(rsys.M, mass)
assert_aae(rsys.Q, -mass * 2.3)
def test_single_rigid_body(self):
mass = 36.2
inertia = np.diag([75.4, 653, 234])
s = System()
j = FreeJoint('joint')
b = RigidBody('body', mass, inertia)
s.add_leaf(j)
j.add_leaf(b)
s.setup()
# Calculate reduced system to get rigid body matrices
rsys = ReducedSystem(s)
self.assertEqual(rsys.M.shape, (6, 6))
self.assertEqual(rsys.Q.shape, (6, ))
assert_aae(rsys.M[:3, :3], mass * np.eye(3))
assert_aae(rsys.M[3:, 3:], inertia)
assert_aae(rsys.M[3:, :3], 0)
assert_aae(rsys.M[:3, 3:], 0)
assert_aae(rsys.Q, 0)
# Now if some freedoms are prescribed, don't appear in reduced system
s.prescribe(j, part=[1, 2, 3, 4, 5]) # only x-translation is free now
rsys = ReducedSystem(s)
self.assertEqual(rsys.M.shape, (1, 1))
self.assertEqual(rsys.Q.shape, (1, ))
assert_aae(rsys.M[0, 0], mass)
assert_aae(rsys.Q, 0)
def rigid_body_mass_matrix(element):
joint = FreeJoint('joint')
system = System()
system.add_leaf(joint)
joint.add_leaf(element)
system.setup()
for el in joint.iter_leaves():
system.prescribe(el, 0, 0)
system.update_kinematics()
rsys = ReducedSystem(system)
return rsys.M
def rigid_body_mass_matrix(element):
joint = FreeJoint('joint')
system = System()
system.add_leaf(joint)
joint.add_leaf(element)
system.setup()
for el in joint.iter_leaves():
system.prescribe(el, 0, 0)
system.update_kinematics()
rsys = ReducedSystem(system)
return rsys.M
class Gyroscope:
def __init__(self, length, radius, mass, spin):
self.length = length
self.radius = radius
self.mass = mass
self.spin = spin
self.bearing = Hinge('bearing', [0, 0, 1])
self.pivot = Hinge('pivot', [0, 1, 0])
self.axis = Hinge('axis', [1, 0, 0])
self.body = self.build_body()
self.pivot.damping = 200
self.system = System(gravity=9.81)
self.system.add_leaf(self.bearing)
self.bearing.add_leaf(self.pivot)
self.pivot.add_leaf(self.axis)
self.axis.add_leaf(self.body)
self.system.setup()
# Prescribed DOF accelerations: constant rotational speed
self.system.prescribe(self.axis)
def build_body(self):
Jx = self.radius**2 / 2
Jyz = (3*self.radius**2 + self.length**2) / 12
Jyz_0 = Jyz + (self.length/2)**2 # parallel axis theorem
inertia = self.mass * np.diag([Jx, Jyz_0, Jyz_0])
return RigidBody('body', self.mass, inertia, [self.length/2, 0, 0])
def simulate(self, xpivot=0.0, vpivot=0.0, t1=10, dt=0.05):
# reset
self.system.q[:] = 0.0
self.system.qd[:] = 0.0
# initial conditions
self.system.q[self.pivot.istrain][0] = xpivot # initial elevation
#self.system.qd[self.pivot.istrain][0] = vpivot # initial elevation spd
self.system.qd[self.axis.istrain][0] = self.spin # constant spin speed
# simulate
integ = Integrator(self.system, ('pos', 'vel'))
integ.integrate(t1, dt, nprint=None)
return integ
class Gyroscope:
def __init__(self, length, radius, mass, spin):
self.length = length
self.radius = radius
self.mass = mass
self.spin = spin
self.bearing = Hinge('bearing', [0, 0, 1])
self.pivot = Hinge('pivot', [0, 1, 0])
self.axis = Hinge('axis', [1, 0, 0])
self.body = self.build_body()
self.pivot.damping = 200
self.system = System(gravity=9.81)
self.system.add_leaf(self.bearing)
self.bearing.add_leaf(self.pivot)
self.pivot.add_leaf(self.axis)
self.axis.add_leaf(self.body)
self.system.setup()
# Prescribed DOF accelerations: constant rotational speed
self.system.prescribe(self.axis)
def build_body(self):
Jx = self.radius**2 / 2
Jyz = (3 * self.radius**2 + self.length**2) / 12
Jyz_0 = Jyz + (self.length / 2)**2 # parallel axis theorem
inertia = self.mass * np.diag([Jx, Jyz_0, Jyz_0])
return RigidBody('body', self.mass, inertia, [self.length / 2, 0, 0])
def simulate(self, xpivot=0.0, vpivot=0.0, t1=10, dt=0.05):
# reset
self.system.q[:] = 0.0
self.system.qd[:] = 0.0
# initial conditions
self.system.q[self.pivot.istrain][0] = xpivot # initial elevation
#self.system.qd[self.pivot.istrain][0] = vpivot # initial elevation spd
self.system.qd[self.axis.istrain][0] = self.spin # constant spin speed
# simulate
integ = Integrator(self.system, ('pos', 'vel'))
integ.integrate(t1, dt, nprint=None)
return integ
class hinged_beam_tests(unittest.TestCase):
density = 5.0
length = 20.0
force = 34.2 # N/m
hinge_torque = 0.0
free_beam = False
def setUp(self):
# FE model for beam
x = linspace(0, self.length, 20)
fe = BeamFE(x, density=self.density, EA=0, EIy=1, EIz=0)
fe.set_boundary_conditions('C', 'F')
self.beam = ModalElementFromFE('beam', fe, 0)
# Set loading - in negative Z direction
load = np.zeros((len(x), 3))
load[:, 2] = -self.force
self.beam.loading = load
# Hinge with axis along Y axis
self.hinge = Hinge('hinge', [0, 1, 0])
self.hinge.internal_torque = self.hinge_torque
# Build system
self.system = System()
self.system.add_leaf(self.hinge)
self.hinge.add_leaf(self.beam)
self.system.setup()
if not self.free_beam:
# Prescribe hinge to be fixed
self.system.prescribe(self.hinge)
# Initial calculations
self.recalc()
def recalc(self):
self.system.update_kinematics() # Set up nodal values initially
self.system.update_matrices()
self.system.solve_accelerations() # Calculate accelerations of DOFs
self.system.update_kinematics() # Update nodal values based on DOFs
self.system.update_matrices()
self.system.solve_reactions() # Solve reactions incl d'Alembert
def test_prescribe_free(self):
s = System()
j = FreeJoint('joint')
s.add_leaf(j)
s.setup()
s.time = 3.54
# Initially all 6 joint motions are free
self.assertEqual(len(s.q.dofs), 6)
# Prescribing joint to be fixed results in no dofs
s.prescribe(j)
self.assertEqual(len(s.q.dofs), 0)
# Freeing joint results in 6 dofs again
s.free(j)
self.assertEqual(len(s.q.dofs), 6)
# Prescribing 2 of joint motions leaves 4 dofs
s.prescribe(j, lambda t: t, [0, 2])
self.assertEqual(len(s.q.dofs), 4)
# Prescribing another joint motions leaves 3 dofs
s.prescribe(j, 2.0, part=3)
self.assertEqual(len(s.q.dofs), 3)
# Check accelerations are applied to qdd
assert_aae(s.qdd[j.istrain], 0)
s.apply_prescribed_accelerations()
assert_aae(s.qdd[j.istrain], [3.54, 0, 3.54, 2.0, 0, 0])
# Freeing joint results in 6 dofs again
s.free(j)
self.assertEqual(len(s.q.dofs), 6)
def test_adding_elements_with_strains(self):
slider = PrismaticJoint('slider', [1, 0, 0])
conn = RigidConnection('conn')
body = RigidBody('body', mass=1.235)
s = System()
s.add_leaf(slider)
slider.add_leaf(conn)
conn.add_leaf(body)
s.setup()
# Should have dict of elements
self.assertEqual(s.elements,
{'slider': slider, 'conn': conn, 'body': body})
# Number of states:
# 6 ground
# + 6 constraints on slider
# + 1 strain in slider
# + 6 <node-0> between slider and conn
# + 6 constraints on conn
# + 6 <node-1> between conn and body
# ---
# 31
self.assertEqual(s.lhs.shape, (31, 31))
for vec in (s.rhs, s.qd, s.qdd):
self.assertEqual(len(vec), 31)
# Check there is the one slider dof
self.assertEqual(len(s.q.dofs), 1)
self.assertEqual(len(s.qd.dofs), 1)
self.assertEqual(len(s.qdd.dofs), 1)
# After prescribing the slider, there should be no dofs
s.prescribe(slider)
self.assertEqual(len(s.q.dofs), 0)
self.assertEqual(len(s.qd.dofs), 0)
self.assertEqual(len(s.qdd.dofs), 0)
def setUp(self):
# Parameters
mass = 11.234
length = 2.54
gravity = 9.81
# Build model
slider = PrismaticJoint('slider', [1, 0, 0])
link = RigidConnection('link', [0, 0, length])
body = RigidBody('body', mass)
system = System(gravity=gravity)
system.add_leaf(slider)
slider.add_leaf(link)
link.add_leaf(body)
system.setup()
# Prescribe motion -- sinusoidal acceleration
motion_frequency = 1 # Hz
motion_amplitude = 2.3 # m
# x = motion_amplitude * np.cos(w*t)
# v = -motion_amplitude * np.sin(w*t) * w
# a = -motion_amplitude * np.cos(w*t) * w**2
def prescribed_acceleration(t):
w = 2 * np.pi * motion_frequency
return -w**2 * motion_amplitude * np.cos(w * t)
system.prescribe(slider, prescribed_acceleration)
# Set the correct initial condition
system.q[slider.istrain][0] = motion_amplitude
system.qd[slider.istrain][0] = 0.0
# Custom outputs to calculate correct answer
def force_body_prox(s):
a = prescribed_acceleration(s.time)
Fx = mass * a
Fz = mass * gravity
return [Fx, 0, Fz, 0, 0, 0]
def force_link_prox(s):
a = prescribed_acceleration(s.time)
Fx = mass * a
Fz = mass * gravity
My = length * Fx
return [Fx, 0, Fz, 0, My, 0]
def force_slider_prox(s):
a = prescribed_acceleration(s.time)
x = -a / (2 * np.pi * motion_frequency)**2
Fx = mass * a
Fz = mass * gravity
My = length * Fx - x * Fz
return [Fx, 0, Fz, 0, My, 0]
# Solver
integ = Integrator(system, ('pos', 'vel', 'acc'))
integ.add_output(LoadOutput(slider.iprox))
integ.add_output(LoadOutput(link.iprox))
integ.add_output(LoadOutput(body.iprox))
integ.add_output(CustomOutput(force_slider_prox, "correct ground"))
integ.add_output(CustomOutput(force_link_prox, "correct slider dist"))
integ.add_output(CustomOutput(force_body_prox, "correct link dist"))
self.system = system
self.integ = integ
def setUp(self):
# Parameters
mass = 11.234
length = 2.54
gravity = 9.81
# Build model
slider = PrismaticJoint('slider', [1, 0, 0])
link = RigidConnection('link', [0, 0, length])
body = RigidBody('body', mass)
system = System(gravity=gravity)
system.add_leaf(slider)
slider.add_leaf(link)
link.add_leaf(body)
system.setup()
# Prescribe motion -- sinusoidal acceleration
motion_frequency = 1 # Hz
motion_amplitude = 2.3 # m
# x = motion_amplitude * np.cos(w*t)
# v = -motion_amplitude * np.sin(w*t) * w
# a = -motion_amplitude * np.cos(w*t) * w**2
def prescribed_acceleration(t):
w = 2*np.pi*motion_frequency
return -w**2 * motion_amplitude * np.cos(w*t)
system.prescribe(slider, prescribed_acceleration)
# Set the correct initial condition
system.q[slider.istrain][0] = motion_amplitude
system.qd[slider.istrain][0] = 0.0
# Custom outputs to calculate correct answer
def force_body_prox(s):
a = prescribed_acceleration(s.time)
Fx = mass * a
Fz = mass * gravity
return [Fx, 0, Fz, 0, 0, 0]
def force_link_prox(s):
a = prescribed_acceleration(s.time)
Fx = mass * a
Fz = mass * gravity
My = length * Fx
return [Fx, 0, Fz, 0, My, 0]
def force_slider_prox(s):
a = prescribed_acceleration(s.time)
x = -a / (2*np.pi*motion_frequency)**2
Fx = mass * a
Fz = mass * gravity
My = length*Fx - x*Fz
return [Fx, 0, Fz, 0, My, 0]
# Solver
integ = Integrator(system, ('pos', 'vel', 'acc'))
integ.add_output(LoadOutput(slider.iprox))
integ.add_output(LoadOutput(link.iprox))
integ.add_output(LoadOutput(body.iprox))
integ.add_output(CustomOutput(force_slider_prox, "correct ground"))
integ.add_output(CustomOutput(force_link_prox, "correct slider dist"))
integ.add_output(CustomOutput(force_body_prox, "correct link dist"))
self.system = system
self.integ = integ
