def optimize_path(start_pos, target_pos_in, name): global target_pos target_pos = target_pos_in dynamics_engine = MyRobotDynamics.cached_init("simulation") dynamics_engine.dt.set_value(0.1) dynamics_engine.loads["drivetrain"].position.set_value(start_pos) dynamics_engine.prediction_func() init_state = dynamics_engine.mean_state.get_value() # optimization problem T = 100 # horizon u0 = .1 * np.random.randn(T, 2) # initial controls # u0 = zeros((T, 2)) # initial controls options = {} # run the optimization options["lims"] = np.array([[-1, 1], [-1, 1]]) start_time = time.time() x, u, L, Vx, Vxx, cost = ilqg.ilqg(dynamics_engine, cost_func, init_state, u0, options) data = {"x": x, "u": u, "Vx": Vx, "Vxx": Vxx, "cost": cost} with open("{}.tdc".format(name), 'wb') as f: pickle.dump(data, f, -1) print(x[-1]) print("ilqg took {} seconds".format(time.time() - start_time))
def robotInit(self): '''Robot-wide initialization code should go here''' self.dynamics = MyRobotDynamics.cached_init("estimation") self.dynamics.init_wpilib_devices() self.stick = wpilib.Joystick(0)
from dynamics import MyRobotDynamics import theano import logging logging.getLogger().setLevel(logging.DEBUG) dynamics = MyRobotDynamics.cached_init("simulation") #theano.printing.pydotprint(dynamics.state_prediction_mean_update, outfile="simulation_update.png", var_with_name_simple=True) dynamics.controllers["left_cim"].set_percent_vbus(1) dynamics.controllers["right_cim"].set_percent_vbus(-1) for _ in range(200): dynamics.simulation_update(.1) print(dynamics.get_state())
from dynamics import MyRobotDynamics import logging logging.getLogger().setLevel(logging.DEBUG) lift = MyRobotDynamics("simulation") for _ in range(300): lift.simulation_update(.06) print(lift.get_state())
from dynamics import MyRobotDynamics import logging logging.getLogger().setLevel(logging.DEBUG) shooter = MyRobotDynamics("simulation") shooter.controllers["shooter"].set_percent_vbus(1) print("\n2 seconds of spinning up. Each frame is half a second.\n") for _ in range(20): shooter.simulation_update(.1) print(shooter.get_state()) shooter.add_ball() print("\nEngaging ball! Assuming instant ball acceleration. Each frame is now one millisecond.\n") for _ in range(8): shooter.simulation_update(.01) print(shooter.get_state())
def get_dynamics(self): return MyRobotDynamics.cached_init("simulation")