def test_1dof_nonlinear_system(self):
s = System()
j = PrismaticJoint('joint', [0, 0, 1])
k = 0.45 # quadratic stiffness coefficient
j.internal_force = lambda el, t: -k * el.xstrain[0]**2
b = RigidBody('body', 10)
s.add_leaf(j)
j.add_leaf(b)
s.setup()
# Linearise around z0 = 0: stiffness should be zero
linsys = LinearisedSystem.from_system(s, z0=0)
assert_aae(linsys.M, [[10.0]])
assert_aae(linsys.C, [[0.0]])
assert_aae(linsys.K, [[0.0]])
# Linearise about z0 = 2: stiffness should be 2kx
linsys = LinearisedSystem.from_system(s, z0=[2])
assert_aae(linsys.M, [[10.0]])
assert_aae(linsys.C, [[0.0]])
assert_aae(linsys.K, [[2 * k * 2]])
# Test setting z0 in another way
linsys = LinearisedSystem.from_system(s, z0={'joint': [4.2]})
assert_aae(linsys.M, [[10.0]])
assert_aae(linsys.C, [[0.0]])
assert_aae(linsys.K, [[2 * k * 4.2]])
def test_1dof_nonlinear_system(self):
s = System()
j = PrismaticJoint('joint', [0, 0, 1])
k = 0.45 # quadratic stiffness coefficient
j.internal_force = lambda el, t: -k * el.xstrain[0]**2
b = RigidBody('body', 10)
s.add_leaf(j)
j.add_leaf(b)
s.setup()
# Linearise around z0 = 0: stiffness should be zero
linsys = LinearisedSystem.from_system(s, z0=0)
assert_aae(linsys.M, [[10.0]])
assert_aae(linsys.C, [[0.0]])
assert_aae(linsys.K, [[0.0]])
# Linearise about z0 = 2: stiffness should be 2kx
linsys = LinearisedSystem.from_system(s, z0=[2])
assert_aae(linsys.M, [[10.0]])
assert_aae(linsys.C, [[0.0]])
assert_aae(linsys.K, [[2 * k * 2]])
# Test setting z0 in another way
linsys = LinearisedSystem.from_system(s, z0={'joint': [4.2]})
assert_aae(linsys.M, [[10.0]])
assert_aae(linsys.C, [[0.0]])
assert_aae(linsys.K, [[2 * k * 4.2]])
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 setUp(self):
joint = PrismaticJoint('joint', [0, 0, 1])
joint.stiffness = self.K
joint.damping = 2 * self.damping_coeff * (self.K * self.M) ** 0.5
body = RigidBody('body', self.M)
system = System()
system.add_leaf(joint)
joint.add_leaf(body)
system.setup()
system.update_kinematics()
self.joint, self.body, self.system = joint, body, system
def test_1dof_linear_system(self):
s = System()
j = PrismaticJoint('joint', [0, 0, 1])
j.stiffness = 5.0
j.damping = 2.3
b = RigidBody('body', 10)
s.add_leaf(j)
j.add_leaf(b)
s.setup()
linsys = LinearisedSystem.from_system(s)
assert_array_equal(linsys.M, [[10.0]])
assert_array_equal(linsys.C, [[2.3]])
assert_array_equal(linsys.K, [[5.0]])
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_find_equilibrium(self):
g = 9.81
m = 23.1
k = 45.2
s = System(gravity=g)
slider = PrismaticJoint('slider', [0, 0, 1])
slider.stiffness = k
body = RigidBody('body', mass=m)
s.add_leaf(slider)
slider.add_leaf(body)
s.setup()
# Initially position should be zero and acceleration nonzero
s.solve_accelerations()
assert_aae(slider.xstrain, 0)
assert_aae(slider.astrain, -g)
# At equilibrium, position should be nozero and force on body zero
s.find_equilibrium()
s.update_matrices() # recalculate stiffness force
s.solve_accelerations()
assert_aae(slider.xstrain, -m * g / k)
assert_aae(slider.astrain, 0)
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):
joint = PrismaticJoint('joint', [0, 0, 1])
joint.stiffness = self.K
joint.damping = 2 * self.damping_coeff * (self.K * self.M)**0.5
body = RigidBody('body', self.M)
system = System()
system.add_leaf(joint)
joint.add_leaf(body)
system.setup()
system.update_kinematics()
self.joint, self.body, self.system = joint, body, system
def test_1dof_linear_system(self):
s = System()
j = PrismaticJoint('joint', [0, 0, 1])
j.stiffness = 5.0
j.damping = 2.3
b = RigidBody('body', 10)
s.add_leaf(j)
j.add_leaf(b)
s.setup()
linsys = LinearisedSystem.from_system(s)
assert_array_equal(linsys.M, [[10.0]])
assert_array_equal(linsys.C, [[2.3]])
assert_array_equal(linsys.K, [[5.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
