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 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):
# optimization problem
T = 100 # horizon
x0 = array([0, 0, 0]) # initial state
u0 = .1*random.randn(T, 2) # initial controls
#u0 = zeros((T, 2)) # initial controls
options = {}
# run the optimization
options["lims"] = array([[-1, 1],
[-1, 1]])
start_time = time.time()
self.x, self.u, L, Vx, Vxx, cost = ilqg.ilqg(lambda x, u: dynamics_func(x, u), cost_func, x0, u0, options)
self.i = 0
print(self.x[-1])
print("ilqg took {} seconds".format(time.time() - start_time))
cost_graph(self.x)
self.drive = wpilib.RobotDrive(0, 1)
self.joystick = wpilib.Joystick(0)
