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')])