sacsys.step()
q = np.vstack((q,
np.hstack(
(system.q[0], system.q[1], system.q[2], system.q[3],
system.q[4], system.q[5], system.q[6]))))
u = np.vstack(
(u, np.hstack((system.u[0], system.u[1], system.u[2], system.u[3]))))
if np.abs(sacsys.time % 1) < dt:
print "time = ", sacsys.time
print(system.q[6])
nn = np.shape(np.array(u))[0]
print(nn)
# pylab.plot(np.linspace(0,tf, num=nn), zip(*q)[system.get_config('Theta_Left').index])
# pylab.plot(np.linspace(0,tf, num=nn), zip(*q)[system.get_config('Theta_Right').index])
pylab.plot(np.linspace(0, tf, num=nn),
zip(*q)[system.get_config('Theta_pax').index])
pylab.plot(np.linspace(0, tf, num=nn), np.pi * np.ones(nn))
# pylab.plot(np.linspace(0,tf, num=nn), 0.*np.pi*np.ones(nn))
# #pylab.plot(np.linspace(0,tf, num=nn), u)
pylab.legend(["Theta_Left", "Theta_Right", "PI"])
pylab.show()
# sacsys.get_final_traj()
# sacsys.save()
# # Visualize the system in action
# visual.visualize_3d([ visual.VisualItem3D(system, np.linspace(0,tf, num=nn) , q) ])
# Create a visualization of the scissor lift.
visual.visualize_2d(
[visual.VisualItem2D(system, np.linspace(0, tf, num=nn), q, plane='XZ')])
pylab.subplot(222, sharex=ax1)
pylab.plot(T, zip(*X)[6])
pylab.plot(T, zip(*X)[7])
pylab.xlabel("T")
pylab.ylabel("pax angle")
pylab.legend(['pax-torque'])
pylab.subplot(223, sharex=ax1)
pylab.plot(T[1:], zip(*U)[0])
pylab.plot(T[1:], zip(*U)[1])
pylab.plot(T[1:], zip(*U)[2])
pylab.plot(T[1:], zip(*U)[3])
pylab.xlabel("T")
pylab.ylabel("U")
pylab.title("Inputs - Lift & Moments")
pylab.legend(['fz', 'm1', 'm2', 'm3', 'pax-torque'])
pylab.subplot(224, sharex=ax1)
pylab.plot(T, zip(*X)[2])
pylab.plot(T, zip(*X)[1])
pylab.plot(T, zip(*X)[0])
pylab.xlabel("T")
pylab.ylabel("Z")
pylab.title("Position in 3D")
pylab.legend(["X", "Y", "Z"])
pylab.show()
visual.visualize_2d([visual.VisualItem2D(system, T, X, plane='XZ')])
visual.visualize_2d([visual.VisualItem2D(system, T, X, plane='YZ')])
visual.visualize_2d([visual.VisualItem2D(system, T, X, plane='XY')])
drop.set_mass(0.0)
wiggle = Frame(drop, trep.TY, "WIGGLE")
wiggle.config.q = 10. #L_link*math.cos(theta_0)
wiggle.set_mass(0.0)
# Add the top slider
slide = Frame(wiggle, trep.TX, "SLIDE")
slide.config.q = 0. #L_link*math.cos(theta_0)
slide.set_mass(0.0)
# Create all the links in the system.
add_level(slide, slide)
# The scissor lift should satisfy the constraints, but call
# satisfy_constraints() in case it needs to be nudged a little
# from numerical error.
trep.forces.Damping(system, 0.0, {
'DROP': dampz,
'WIGGLE': dampy,
'SLIDE': dampx
})
system.satisfy_constraints()
return system
# Create the scissor lift
system = falling_links()
# Simulate the system
(t, q) = simulate_system(system)
# Create a visualization of the scissor lift.
visual.visualize_2d([visual.VisualItem2D(system, t, q, plane='XZ')])
