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
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 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