def simulate(system):
# Set initial conditions
system.q[system.elements['hinge'].istrain][0] = 0.3
# Integrate
integ = Integrator(system, ('pos','vel','acc'))
t, y = integ.integrate(5, 0.01, nprint=None)
return integ
def simulate(system):
# Set initial conditions
system.q[system.elements['hinge'].istrain][0] = 0.3
# Integrate
integ = Integrator(system, ('pos', 'vel', 'acc'))
t, y = integ.integrate(5, 0.01, nprint=None)
return integ
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
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
def test_callback(self):
s = System()
s.setup()
# Exponential decay: qd = -A q
def callback(system, ti, q_struct, q_other):
self.assertIs(system, s)
self.assertEqual(len(q_other), 1)
return -q_other
integ = Integrator(s)
t, y = integ.integrate(9.0, 0.1, extra_states=np.ones(1),
callback=callback)
# 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))
assert_aae(y[0][:, 0], np.exp(-t))
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 setUp(self):
# Parameters
mass = 11.234
length = 2.54
gravity = 9.81
# Build model
hinge = Hinge('hinge', [0, 1, 0])
link = RigidConnection('link', [length, 0, 0])
body = RigidBody('body', mass)
system = System(gravity=gravity)
system.add_leaf(hinge)
hinge.add_leaf(link)
link.add_leaf(body)
system.setup()
# Custom outputs to calculate correct answer
def force_body_prox_local(s):
theta = s.q[hinge.istrain][0]
thetadot = s.qd[hinge.istrain][0]
thetadotdot = s.qdd[hinge.istrain][0]
Fx = mass * (-gravity * np.sin(theta) - length * thetadot**2)
Fz = mass * (+gravity * np.cos(theta) - length * thetadotdot)
return [Fx, 0, Fz, 0, 0, 0]
def force_hinge_prox(s):
theta = s.q[hinge.istrain][0]
thetadot = s.qd[hinge.istrain][0]
thetadotdot = s.qdd[hinge.istrain][0]
A = np.array([[+np.cos(theta), np.sin(theta)],
[-np.sin(theta), np.cos(theta)]])
Fxz = -mass * length * np.dot(A, [thetadot**2, thetadotdot])
return [Fxz[0], 0, Fxz[1] + gravity * mass, 0, 0, 0]
# Solver
integ = Integrator(system, ('pos', 'vel', 'acc'))
integ.add_output(LoadOutput(hinge.iprox))
integ.add_output(LoadOutput(link.iprox))
integ.add_output(LoadOutput(body.iprox))
integ.add_output(LoadOutput(body.iprox, local=True))
integ.add_output(CustomOutput(force_hinge_prox, "correct ground"))
integ.add_output(
CustomOutput(force_body_prox_local, "correct link distal local"))
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
def integrate(system, callback=None):
integ = Integrator(system)
t, y = integ.integrate(10, 0.5, nprint=None, callback=callback)
return y[0]
def integrate(system, callback=None):
# dopri5 gives exact result for step response
integ = Integrator(system, method='dopri5')
t, y = integ.integrate(10, 0.5, nprint=None, callback=callback)
return t, y[0]
def integrate(system, callback=None):
integ = Integrator(system)
t, y = integ.integrate(10, 0.5, nprint=None, callback=callback)
return y[0]
def integrate(system, callback=None):
# dopri5 gives exact result for step response
integ = Integrator(system, method='dopri5')
t, y = integ.integrate(10, 0.5, nprint=None, callback=callback)
return t, y[0]
def setUp(self):
# Parameters
mass = 11.234
length = 2.54
gravity = 9.81
# Build model
hinge = Hinge('hinge', [0, 1, 0])
link = RigidConnection('link', [length, 0, 0])
body = RigidBody('body', mass)
system = System(gravity=gravity)
system.add_leaf(hinge)
hinge.add_leaf(link)
link.add_leaf(body)
system.setup()
# Custom outputs to calculate correct answer
def force_body_prox_local(s):
theta = s.q[hinge.istrain][0]
thetadot = s.qd[hinge.istrain][0]
thetadotdot = s.qdd[hinge.istrain][0]
Fx = mass * (-gravity*np.sin(theta) - length*thetadot**2)
Fz = mass * (+gravity*np.cos(theta) - length*thetadotdot)
return [Fx, 0, Fz, 0, 0, 0]
def force_hinge_prox(s):
theta = s.q[hinge.istrain][0]
thetadot = s.qd[hinge.istrain][0]
thetadotdot = s.qdd[hinge.istrain][0]
A = np.array([[+np.cos(theta), np.sin(theta)],
[-np.sin(theta), np.cos(theta)]])
Fxz = -mass * length * np.dot(A, [thetadot**2, thetadotdot])
return [Fxz[0], 0, Fxz[1] + gravity*mass, 0, 0, 0]
# Solver
integ = Integrator(system, ('pos', 'vel', 'acc'))
integ.add_output(LoadOutput(hinge.iprox))
integ.add_output(LoadOutput(link.iprox))
integ.add_output(LoadOutput(body.iprox))
integ.add_output(LoadOutput(body.iprox, local=True))
integ.add_output(CustomOutput(force_hinge_prox, "correct ground"))
integ.add_output(CustomOutput(force_body_prox_local,
"correct link distal local"))
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