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):
'''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")
