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')])
while sacsys.time < tf:
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')])
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')])
dsys_b.set(X[k], U[k], 0)
else:
dsys_b.step(U[k])
X[k + 1] = dsys_b.f()
# Generate a new cost function for the current system.
qd = generate_desired_trajectory(system, t, 130 * mpi / 180)
(Xd, Ud) = dsys_b.build_trajectory(qd)
Qcost = make_state_cost(dsys_b, 0.01, 0.01, 100.0)
Rcost = make_input_cost(dsys_b, 0.01, 0.01)
cost = discopt.DCost(Xd, Ud, Qcost, Rcost)
optimizer = discopt.DOptimizer(dsys_b, cost)
# Perform the optimization on the real system
optimizer.first_method_iterations = 4
finished, X, U = optimizer.optimize(X, U, max_steps=40)
if '--novisual' not in sys.argv:
q, p, v, u, rho = dsys_b.split_trajectory(X, U)
if False:
view = Viewer(system, t, q, qd)
view.main()
else:
visual.visualize_2d([
PendCartVisual(system, t, qd),
PendCartVisual(system, t, q, draw_track=False)
])
X[k+1] = dsys_b.f()
# Generate a new cost function for the current system.
qd = generate_desired_trajectory(system, t, 130*mpi/180)
(Xd, Ud) = dsys_b.build_trajectory(qd)
Qcost = make_state_cost(dsys_b, 0.01, 0.01, 100.0)
Rcost = make_input_cost(dsys_b, 0.01, 0.01)
cost = discopt.DCost(Xd, Ud, Qcost, Rcost)
optimizer = discopt.DOptimizer(dsys_b, cost)
# Perform the optimization on the real system
optimizer.first_method_iterations = 4
finished, X, U = optimizer.optimize(X, U, max_steps=40)
if '--novisual' not in sys.argv:
q,p,v,u,rho = dsys_b.split_trajectory(X, U)
if False:
view = Viewer(system, t, q, qd)
view.main()
else:
visual.visualize_2d([
PendCartVisual(system, t, qd),
PendCartVisual(system, t, q, draw_track=False)
])
trep.constraints.PointOnPlane(system, left_mid, (0,0,1), right_mid)
# Add a new level. Note that left and right switch each time
return add_level(right_end, left_end, link+1)
# Create the new system
system = trep.System()
trep.potentials.Gravity(system, name="Gravity")
# Add the top slider
slider = Frame(system.world_frame, trep.TX, "SLIDER")
slider.config.q = L_link*math.cos(theta_0)
slider.set_mass(m_slider)
# Create all the links in the system.
add_level(system.world_frame, slider)
# The scissor lift should satisfy the constraints, but call
# satisfy_constraints() in case it needs to be nudged a little
# from numerical error.
system.satisfy_constraints()
return system
# Create the scissor lift
system = make_scissor_lift()
# 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')])
